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ABS*^iUCT 


A full sik>dfgrf«-<rf'frfedom computer model of the Naval Poetgraduatt 
School Autonomous Underwater Vehicle (NPS AUV tl) is developed. 
Hydrodynamic Coefficients are determined by geometric similarity %vith an 
existing swimmer delivery vehicle and analysis of initial open loop AUV 11 trials. 
Comparisons between si»nulated and eiKperimental results demonstrate die 
validity of the model and the techniques used. A reduced ordtr observer of lateral 
velocity was produced to provide an input for an enhanced position estimator. 
Results are presented which show that the position estiirator can be calibrated 
using AUV 11 run data to provide a real-time accurate estimate of position. 
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I. INTRODUCTION 

A. GENERAL 

During th€ p«st three decides there hit been in increiting interest by the 
U.& Nivy in the use ot unmirmed underwiter vehicles (UUVs) fRef. 1]. 
These vehicles cm be either tethered vehicles (TUVs), controlled by i cible, or 
completely lutonomous (AUV). Beginning in mid 1960s the US. Nivy his used 
m AUV mc^Ue submirine simulitor (MOSS) is i subnidnc decoy on biHisdc 
missile submarines In 1988 the U S. Nivy md Charles Stark Draper Liboritoty 
in Cambridge, Massachusei^s, initiated i study to determine how UUVs could be 
employed to meet specific Navy missions. IRef. 2] 

Within the Navy, possible missions for AUVs include submirine, anti¬ 
submarine warfare wherein the AUV could conduct surveillmce of or act is i 
decoy to enemy vessels. In a mine warfare roll m AUV atuld be employed to 
map an enemy mine field to provide fi*ie*'''11y forces with information they could 
use to find a safe trmsit of the field. AUVs could be employed to conduct 
surveillance of harbor activity using a variety of sensors. 

Applications of AUV technology are not limited to military missions. AUVs 
could be programmed to explore areas of the ocean where manned vehicles 
cannot travel, or where their endurance is limited by fuel and or food supplies. 
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AUVs could be used in SAlvege operations to inspect the area prior to employing 
manned submersibtes. 

Interest in the applications of AUV technology is evidenced by the growing 
number of conferences and wc. ksh'^ps dedicated to the subject [Ref. 3J 
[Ref. 4). The design and operation of AUVs present unique challenges due 
to the vessel's required r.bility to operate without human intervention. The 
control, giiidance, and mission control software architectvires are exceptionally 
complex and represent the state-of-the-art in real-time "intdligent" compute 
control software design. 

B. OVERVIEW OF NAVAL POSTGRADUATE SCHOOL AUV PROJECT 

The AUV program at the Naval Postgraduate School (NPS) began in 1987 
with the sponsorship of the Naval Surface Weapons System [Ref. 5]. The 
project is a joint effort of the Mechanical Engineering, Computer Science, and 
Electrical and Computer Engineering Departments. The research ’nvolves an 
integrated approach to mission planning and execution including navigation, 
collision avoidance, obstacle recognition, vehicle dynamics and control, and real¬ 
time onboard control software. Within the Mechanical Engineering Department 
efforts have focused in the areas of vehicle configuration and construction, vehicle 
dynamics and control, with experimental and computer simulation fRef. 6]. 
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1. vehicle Configuration and Construction 

The first autonomous underwater vehicle built at NPS, AUV NPS I, was 
built and studied by Brunner [Ref. 7]. In addition to designing and building 
a vehicle, tlfC use of sensor devices such as gyros, inerfial sensors, and pressure 
cells to measure vehicle performance were investigated. Brunner developed a 
technique to obtain required vehicle performance data from the vehicle, and 
designed a control system that could be employed to test depth changes of the 
vehicle in a testing tank. 

The AUV I was a 30-inch long, seven-inch wide, and four-inch high, 
self-propelled, remotely controlled vehicle. The small size was a constraint 
imposed by available testing facilities. Onboard the vehicle were rate gyro sensors 
for ptch and yaw, pressure cells to measure speed and depth, two DC motors to 
po%ver the two propellers, and a data acquisition system. Size restrictions required 
the use of a tether to transmii control signals and provide power from an external 
power source. The AUV I testing program successfully showed the feasibility of 
developing a controller that would provide accurate depth keeping control of an 
autonomous vehicle. 

The second generation autonomous underwater vehicle bxiilt at NFS 
was designed by Good [Ref. 8). The larger AUV n was designed using 
Total Ship Systems Engineering techniques. This integrated approach involved an 
iterative (design spiral) approach with the following subsystems: Hull, Energy 
Storage and Power Plant, Vehicle Motion Control, Sensor Suite, Obstacle 
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AvoidAnct, Navigation and Guidance, Mi»sion Planning, and Machinery 
Monitoring. Figure 1.1 ahowf the configuration of the AUV n and internal 
equipment arrangetnet\t. 

The AUV n contains its own power supply of rechargeable batteries, 
and an onboard ccmiputer that can be programmed prior to test runs. The larger 
si 2 e has eliminated the need for an external tether. In-water testing of the AUV 
n in the NPS swimming pool began in March 19911. The basic hull performance 
characteristics predicted by Good have been validated. The 7000 ft*, TVi ft deep 
swimming pool enables relatively complex test runs to be performed to test 
control methods and sensor systems. 

2. Vehicle Control 

The AUV guidance system consists of an autopilot and associated 
gtiidance law. The auto pilot is responsible for stabilizing vehicle motion 
dynamics in terms of speed, course, and depth. The guidance law will combine 
commands for the path or position for the vehicle to achieve with navigational 
estimates of true position and orientation to generate speed, course, and depth 
commands for the autopilot. 

Boncal [Ref. 9l investigated the use of a model bised contrcdler 
for accurate path keeping of an underwater vehicle. The Swimmer Delivery 
Vehicle (SDV Mark 9) [Ref. 10] model hydrodynamic coefficients were 
used in this study. Linearized equations of motion were used as the bases for die 
feedforward and feedback elements of a depth controller. Results demonstrated 
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Fig:ttre 1.1 AUV IJ, Transparent View Showing Internal Equipment 
Arrangement (Dra%vn by David Marco) 
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that, ibr depth charging maneuvers, accurate tracking of the planned path could 
be achieved for a ^-onsiderable range of speeds. 

McDonald IRef. 11J combined the work of Boncal in controller 
design and applied it to the AUV1 model. Only vehicle depth was available from 
the AUV I data acquisition system. Consequently, both state estimation of pitch 
and pitch rate and disturbance estimadon/oompensation techniques were used. 
A successful closed loop controller was developed using these methods. Sur 
[Ref. 12], continuing research on the AUV I depth control problem 
designed a sliding-mode compensator for depth control. Computer simulations 
using a full six-degree-of-freedom model and non-lineiu' equations proved the 
method was successfully able to pro\tide accurate depth control for an 
autonomous underwater vehicle. 

Lienard [Ref. 13] demonstrated that sliding mode control 
provides a robust controller for underwater vehicles. Because the hydrodynamic 
forces on a vehicle can not be precisely measured problems may occur in 
predicting and controlling vehicle motion. Using the SDV Mark 9 as the base 
vehicle, Lienard used independent control of linearized motion equations for 
longitudinal and vertical planes, and coupled them together. Lienard used a Line- 
of-Sight guidance scheme. In this method the onboard navigator generates a 
geographic "way point” ahead of the AUV, and then aims the AUV at this point, 
and attempts to drive through it. By successive use of way points the AUV 
proceeds to its destination. Though the navigation control law can't be verified 
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to be stable, the tracking system as a whole is very robust This control scheme 
will place the vehicle on the desired way point, but it may not always be going 
in the direction desired. 

Building on this, Papoulias and Healey [Ref. 14} and Chism 
[Ref. 15] investigated a guidance scheme in which cross-track error is 
nunimized. This method is analogous to driving an automobile down a roadway; 
the goal is to stay in the traffic lane, i.e., minimize deviation from the intended 
track. In this method, the navigator senses the lateral location of the AUV relative 
to the desired track between two way points. 

This method of control is also robust, and generally keeps the vehicle 
closer on track "han the LOS method. The tradeoff to staying close to the ordered 
track at all times is a lot of small maneuvering by control surfaces/thrusters. If, 
a nvunber of way points are located too close, as might happen if the vehicle has 
to maneuver into a harbor, the vehicle response may become excessively 
oscillatory. 

The control schemes described so far approached the AUV control 
problem as a single input/single output (SISO) system. Hawkinson 
[Ref. 16] approached the AUV control problem by applying multiple 
input/multiple output (MEMO) sliding mode control theory. Use of a MIMO 
control method combined both a LOS and cross track error steering controller 
with a linear quadratic regulator for the depth controller. The speed controller 
developed by Lienard was also used. 
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Using the SDV Mark 9 vehicle characteristics, Hawkinson proved the 
superior performance of the MIMO sliding mode controller as compared to SISO 
controllers. Though the depth, speed and steering controllers were designed 
separately, the effectively simultaneously controlled the vehicle. 

Papasotiriou (Ref. 17] investigated the use of a "moving aim 
point" or pursuit autopilot control scheme. This method is also similar to driving 
down the road wherein the driver is actually aiming at a point at some finite 
distance in front of the vehicle. Similarly, with this method the AUV n is always 
driving toward a moving aim p)Oint a few ship lengths ahead. The moving aim 
point is traveling down the line-of-sight toward the next way point. 

This control scheme is almost as robust as the Lin^f-Sight method. 
Both the guidance and control schemes must be designed together in order to 
avoid a loss of stability. This method has an advantage over the Line-of-Sight 
method in that it keeps the, vehicle closer to the track. 

Clothier [Ref. 18] investigated the application of a cubic spiral 
guidance method to autonomous vehicle guidance. This method minimizes cross 
track errors subject to the rate of change of path curvature. Heading commands 
are generated based on the cross track error, the path curvature rate, and the 
difference between desired and actual vehicle heading. This method has the 
advantage of placing the vehicle at a given waypoint at a certain heading. 
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3. Associated Initiatives 

Farren (Ref. 19] and Lohrhammer [Ref. 20] performed 
preliminary work on the design of the AUV II sonar system. The AUV II is 
capable of detecting obstacles in its path, and determining an appropriate 
avoidance course. A high-resolution sonar suite employing four ultrasonic 
transducers has been installed and successfully tested on the ALA^ II by Floyd 
(Ref. 21]. 

To permit precise positioning the AUV II will employ four tube 
thrusters. Two will be vertically oriented, and two will be installed athwartships. 
Research on thrusters for the AUV II was begun by Saunders [Ref. 22] and 
continued by McLean [Ref. 23]. Thrusters are scheduled to be installed and 
tested in the AUV II during the first half on 1992. 

A significant amount of research has been conducted by the Computer 
Science and Computer and Electrical Engineering Departments in the areas of 
mission planning, situation assessment through artificial intelligence, fuzzy logic 
controllers, and fault tolerant controllers. Appendix D contains a bibliography of 
all theses associated with the AUV research program. 

C SCOPE OF THIS THESIS 

This research makes the transition from motion control research based on 
the AUV I and SDV Mark 9 vehicles to a combination of computer simulation and 
experimental investigation into the vehicle dynamics of the AUV II. The initial 
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focus of this project was the development of a lateral motion control law for use 
in initial closed loop operation of the AUV II. Chapter II describes how the 
hydrodynamic coefficients in the yaw and sway motion equations for the AUV 
n were determined. Chapter m discusses the refinements to the initial 
hydrodynamic coefficient estimates, and presents the development of the control 
law used in the early stages of AUV n trials. 

Computer simulations will play a significant role in furtl.er research on the 
AUV n. To enable new control and guidance laws to be tested and evaluated 
without actually using the AUV II, a full six-degreeof-freedom computer model 
of the AUV n was developed. Chapter IV describes how this model was 
developed, and compares the model performance to actual results. 

The final phase of this project involved the design of an erthanced position 
estimation algorithm. Instrumentation and size restrictions linut the amount of 
motion information able to be determined on board the AUV H. Determining a 
vehicle's lateral velocity is a key factor in precise position estimation. Chapter V 
discusses the det’elopment of the reduced order observer for lateral velocity. The 
performance of the enhanced position estimator is compared against simulated 
and experimental AUV n trials. Finally, Chapter VI presents recommendations for 
further research and refinement of the six-degree-of-freedom computer model and 
position estimator for the AUV H. 
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II. LATERAL MOTION HYDRODYNAMIC 
COEFFICIENT DEVELOPMENT 

A. GENERAL LATERAL MOTION EQUATIONS 

The design of the AUV n rontrol system begins with identifying the 
equations of motion. The generalized equations of motion for lateral motion and 
yaw for a submerged vehicle [Ref. 10] are shown in Equations (2.1) and (2.2). 
Variables are referenced to a right-hand orthogonal axis system fixed in the body 
center as shown in Figure 2.1. Table I defines the parameters used; a dot () over 
a quantity indicates a derivative with respect to time. Because the AUV n has two 
rudders, the equations of motion separately accotmt for their effect. 

m[vur - wp ♦ Xgfpqf) - yc(p * + r *) + Zcfqr - p)] * 

- |l‘[Y,(..Y^.Y„pq.Y„qr] 

* |P[Y.*.Y,up.Y,ur.Y^vq.Y^wp.Y„wr] 

♦ |l*tY.uv.Y„vw.Y^,u*Srt,*Y^_u*8J <2-l) 

11“-(C„^(*Xv.xrf .C„,b(xKw-xq«il:li^dx 

2*^- U^x) 

(W-BlcosOsinp 

Sway Equation of Motion 
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z 


Figure 2.1 AUV II Axis System 


I,f^(Iy-I,)pq-I^(p*-q*)-ypr><i) + 
I„(qr - p) +m[xG(v + ur - wp) -ycCu - vr+wq)] = 

A 

+ £.l*[N^v+NpUp-^N,ur+Ny^vq+N„pWp+N^wr] 
®[N^uv+N^vw+Nj^u *5„+Nj^u 
- f’^[CDh(x)(v^xr)2^CD,b(x)(w-xq)2]il::^x^ 

2 *^-(1 U^x) 

+ (xGW-XBB)cos0sin(J) + (yGW-yBB)sine + 

iS 

Yaw Equation of Motion 
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TABLE I. EQUATIONS OF MOTION VARIABLES 


VarUble 

Detoiption 


Distance along the principal axes 

u,v,w 

Velocity components of body axis system relative to fluid 
along t^y axes 


Angular velocity components of body relative to inertial 
reference system along body axes 

X,Y^ 

Hydrodynamic force components along body axes 

K,M,N 

Hydrodynamic nnoment components along body axes 


Yaw, pitch, and roll angles (Euler angles) 

m 

Mass of the AUV 11 (including the fluid in the floodable 
sonar dome) 

w 

Weight of the AUV 11 (^gm) 

▼ 

Displacement volume of the AUV 11 

B 

Buoyancy force acting on the AUV 11 (=gpv) 

xc,yc,zc 

Coordinates of the Center of Gravity in the body axis 
system. These depend on the mass distribution of the 
vehicle 


Coordinates of the Center of Buoyancy in the body axis 
system. These are independent of the mass distri^tion of 
the vehicle 


Moments of inertia about the body system axes 


Products of inertia about the body system axes 

P 

Mass density water 

! 

Reference length used to nondimensionalize the 
hydrodynamic coefficients 

b(x), h(x) 

Width and height of the AUV II in the xy and xz planes, 
respectively, measured in the body axis system shown in 
Figure (2.1) 


Coordinates of the vehicle nose and tail as measured in 
body axis system 

U^x) 

TotaV crossflow velocity on AUV II at position x 


Bow and Stem rudder deflection angles in radians 

^Dy'^Di 

Drag coefficients along the y and z axes of the body system 

■ 

axes. 
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B. LINEAR, SIMPLinED LATERAL MOTION EQUATIONS 

The general equations of motion are extremely non-linear whichi makes their 
direct use in developing a control law very difficult. By using the following 
assumptions and specific physical characteristics of the AUV n, the equations 
were linearized and simplified: 

1. The angiilar velocities about the x-axis (p) and jr-axis (q) are zero. 

The associated accelerations p and are also zero. 

2. The AUV II is neutrally buoyant: W=B. 

3. The AUV D is symmetrically loaded in the transverse and 
longitudinal directions: yt, x^, and x^ are zero. 

4. The counter-rotating propellers produce no yaw moment (Np„p=0). 

5. The non-linear drag force term is small in value compared to other 
terms in the equation, and can be eliminated. For hovering analysis 
this term will have to be included in the simplified equations of 
motion. 


Equations (2.3) and (2.4) are the simplified, lincrar equations of motion used 
for the initial development of the lateral motion control law. 
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2 


£PY,v ^ £l*Y,ur ♦ 

2 2 

£1>Y.uv ♦ £1'Y^.u>8„ . £PY^u>5h. 


(2^) 


Linear, Simplified Sway Equation of Motion 


♦ mxc^ ♦ mx^ur* £i“n^ ♦ 


£l%v * £l%ur > 

2 '' 2 ' 

£PNuv > £i%u* 5„ + £l®N.u»8rt, 
2 2 *" 2 ■ * ™ 


(2.4) 


Linear, Simplified Yaw Equation of Motion 


C DETERMINATION OF HYDRODYNAMIC COEFFICIENTS 

The next step in developing the lateral motion control law was th^ 
determination of hydrodynamic coefficients (Y^, Y^, Y^, N^, N^, N^, and N^) 

appearing in Equations (2.3) and (2.4). The coefficients for the SDV were used as 
a starting point. Though similar in geometry to the AUV n, the SDV has a large 
fin on the stem in which a third propeller for surfaced operations is located. The 
effect of this fin on the hydrodynamic coefficients was estimated and subtracted 
from the original SDV hydrodynamic coefficient values. 
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Table D lists the SDV hydrodynamic coefficients, the calculated fin effects, 
and the final "Finless SDV" values. Computations and details of the fin effect on 
the hydrodynamic coefficients are contained in Appendix A. 


TABLE II. SDV HYDRODYNAMIC CO 

EFnaENTS 

r“ 

Given SDV 

Fin Effect 

Finless SDV 

n 

-0.05550 

-0.01%5 

-0.03585 


-0.09310 

-0.01660 

-0.07650 

EH 

0.00124 

0.00756 

-0.00633 

Hi 

0.02970 

0.00639 

0.02331 ' 

N. 

0.00124 

0.00756 

-0.00632 

N 

V 

-0.00742 

0.00639 

-0.01381 

N 

r 

-0.00340 

-0.00291 

-0.00049 

N 

r 

-0.01640 

-0.00246 

-0.01394 


The signs of the hydrodynamic coefficients Y^, Y^, N^, and depend upon 
whether the vehicle is bov/ or stem dorrinant. The SDV, with the large fin at the 
rear, is stem dominant The signs of all the SDV hydrodynamic coefficients except 
for and agree with predicted values [Ref. 24]. and Y^ should, 
be small numbers, either positive or negative. should be positive, but the 
given value of for the SDV is negative. 

The hydrodynamic coefficients of the bow-dominant Finless SDV all have 
signs consistent with theoretical predictions. Y^ remains positive, but has a 
smaller magnitude. Y^ and become positive. remains negative, which it 
should be for a bow-dominant vehicle. 
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The Finless SDV still differs geometrically from the AUV 11. Table HI 
summarixes the geometric characteristics of the Finless SDV and the AUV 11. Due 
to the significant differences in geometric characteristics, a better estimate of the 
AUV 11 hydrodynamic cosffficientr was needed. 

TABLE III. HNLESS SDV AND AUV GEOMETRIC CHARACTERISTICS 



Finless SDV 

AUV 11 

Length (L) 

209.1’ 

87.625 

Draft (T) 

31.8 

10.125 

Beam (B) 

75.7 

16.25 

c. 

0.77 

0.83 

T/L Ratio 

0.1514 

0.1155 

B/L Ratio 

0.3713 

0.1854 

B/T Ratio 

2.4528 

1.6049 


’ Actual SDV length IS 229.0 inches, but the characteristic length 
used in all calculations is 209.1. Actual and characteristic length for 
the AUV II IS 87.625 inch«. 


Clarke, Gedling, and Hine [Ref, 25] used a multiple regression 
analysis to estimate a marine vehicle's hydrodynamic coefficients. Thirty-six sets 
of data from rotating arm experiments and 36 sets of data from planar motion 
experiments were obtained, and then normalized using (T/L)*. The predictor 
variables used were: Cg, l./B, L/T B/L, T/L, and T/B. Only the terms which 
tended to zero were used in the regression to assure stability. The resulting 
equations for the hydrodynamic coefficients are: 
























Hydrodynamic Coefficient Regressiotk Equation* 

The hydrodynamic coefficient values obtained for the Finless SDV using the 
regression equations are listed in the second column of Table IV as "Regression 
SDV." The results obtained for the AUV 11 are listed under "Regression AUV II." 
All of the hydrodynamic coefficients of the Regression SDV are negative wcept 
for N^, which also should be negative for a bow-dominant vehicle. All the 
hydrodynamic coefficients of the Regression AUV n are negative. 
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TABLE IV. DETERMINATION OF AUV II 
HYDRODYNAMIC COEFRaENTS 



Finlcvt 

SDV 

RegrcMlon 

SDV 

SDV/ 

FiniMtSDV 

(Q>nv«rsk>n) 

Rcgmtion 
AUV II 

RtgrcMion 
AUV 11/ 

Convtraion 

m 

.0.03S«S 

•0.04551 

1.269 

-004353 

•0.03430 

B 

-0.07650 

-0.12624 

lASO 

-0.06430 

-0.03896 


•0.00633 

•0.01632 

2578 

•0.00486 

•0.00189 

B 

0.02331 

-0.00775 

0332' 

-0.00925 

unreliable 

N. 

•0 00633 

-0.02192 

3.463 

-0.00579 

•0.00167 

Nv 

-001381 

•0.06309 

4368 

jPQ^HIIIIIIII 

•0.00714 

N. 

-0.0(1049 

000039 

0.73469 

-0.00188 

unreliable 

IB 

-0.01394 

-0.01020 

0.73171 

-0,00875 

-0.01196 


' Due to inconsistencies between expected and actual hydrodynamic coefficient values for Y^ and 
these conversion factors were not used to obuin estimates for the ALA^ II values as explained in 
the ^t. 


The ratios between the hydrodynamic coefficient values of the Hnless SDV 
and Regression SDV (Table IV, column 3) were used as scaling factors to adjust 
the corresponding Regression AUV n values. The resulting AUV n hydrodynamic 
coefficients are listed in column 5 of Table IV. Because the values of Y andN 

r t 

for the Hnless SDV and the Regression SDV were not consistent, a different 
method to determine the AUV II values was used. 

As shown in Appendix A, the effect of a fin on hydrodynamic coefficients 
is the same for and Y^. The ratio between the Finless SDV and AUV U/ 
Conversion values of Y^, 1.9641, was divided into the Hnless SDV value of Y^ to 
dbtain an estimate of Y^ for the AUV n. As was observed for the Hnless SDV, the 
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sign of is positive, though a negative value had been expected based on an 
assumed bow dominant vessel geometry. Similarly, the effect of a fin is the same 
for and Y^. The ratio between the Finless SDV and AUV Il/Conversion values 
of Y^, 1.05, was divided into the Finless SDV value of to obtain an estimate 
of N, for the AUV II. 

An additional modification to the initial estimates of the AUV n 
hydrodynamic coefficients w’as made. The given SDV values of Y^ and were 

, ' I 

the same, 0.00124, as were the calculated fin effect corrections. It was therefore 
reasonable to assume that the values of Y^ and for the AUV II should be the 
same. The AUV Il/Conversion values for and were averaged, and the 
average was used for the Y^ and AUV II Estimate 1 values. 

D. DETERMINATION OF RUDDER FORCE HYDRODYNAMIC 
COEFHaENTYj 

The y-direction component of the total rudder force is 

Y,5 = Y^-±(L«.83,.D8inp,) (2.6) 

where p, = rudder drift angle 

L = rudder lift 

D = rudder drag 

This expression assumes there is no interaction between the pressure field around 
the rudder and the adjacent ship. In most cases there is a significant interaction 
which results in the total y-direction force on the vessel being larger than 
predicted by Equation (2.6). By expressing, the lift and drag forces in 
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nondimensional form, standard figures can be used for a given shape/section of 
rudder. The nondimensional forms of lift and drag forces are given by Equations 
(2.7) and (2.8). 

Lift Coefficient Ct - (2.7) 

(p/2)Aru2 

Drag Coefficient Cn»-5- (2.8) 

(P/2)AtU* 

where Aj = control surface profile area. 

The rudders on the AUV n are of a NACA 0015 foil section. Figure 2.2 
shows the characteristics of a NACA 0015 section. By using this figure. Cl was 
determined to be 3.15155, where 8 is the rudder deflection in radians. For small 
drift angles CpSinCP,) is much smaller in magnitude than ClCOS(P,). Eliminating the 
drag term from Equation (2.6), approximating cos(3,)=l, and writing the total 
rudder force in nondimensional form yields 

feVu'Y.S-LcosS . 

( 2 / 

Solving for Yj, with 1=87.625 inches, and AT=28.57in^, yields Y8=0.01173. Because 
there are two identical control surfaces at both forward and aft rudders, the actual 
value of Yj for the AUV n is 0.02345. 


2 


*3.15155 
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E. AUV II GEOMETRY AND ESTIMATE/COEFFiaENT SUMMARY 

Table V summarizes the AUV II Estimate ThydrcxJynamic coefficient and 
geometric properties used to in the initial open and closed loop simulations. 
The mass moment of inertia, I^, was calculated iising the equation 

^(B*+L*) (2.10) 

12 

Equation (2.10) assumes that the mass within the vehicle is distributed 
homogeneously. To account for the actual non-homogeneous mass distribution. 
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for the SDV was calculated using Equation (2.10), and the ratio between the 
SDV computed and actual I, [Ref. 10] values was applied to the computed AUV 
II Ij value to obtain the number appearing in Table V. 


TABLE V. AUV II GEOMETRIC PROPERTIES AND ESTIMATE 1 
HYDRODYNAMIC COEFHCIENTS 


m 

-0.03430 

m (slugs) 

435/32.17 1 


-0.03896 

Izdbrft-s^) 

45 1 

m 

-0.00178 

p (slugs/ft^) 

1.94 

m 

0.01187 

Xb (ft) 

+0.125/12 

N. 

V 

-0.00178 


-0.377 

N 

V 

-0.00714 


0.283, 

N, 

-0.00047 

^reference (f^) 

87.625 


-0.01196 

N8,s=Xt,Y5„ 


ilQI 

0.02345 



^6tb 

0.02345 


■BHUlHHi 


* X„ and Xrt, are expressed in fraction of vehicle length. 


23 







































III. INITIAL AUV II CLOSED LOOP SURFACED OPERATION 

A. STATE-SPACE SYSTEM REPRESENTATION 

The state of a dynamic system, such as the AUV H, is defined by a set of 
physical quantities that uniquely determine the condition of the system. The state- 
space approach uses only dynamic variables and their first derivatives with 
respect to time. Thus, the condition of a physical system can be described with 
a set of first order differential equations. 

The general form of state-space system representation is: 


X = Ax -*• Bu 


y = Cx 


(3.1) 


where x = 
u = 

y = 

A = 
B = 
C = 


state vector (nxl) 
external input vector (rxl) 
output/observation vector (mxl) 
open loop dynamic matrix (nxn) 
control distribution matrix (nxr) 
output/calibration matrix (mxn) 


The state vector chosen for the AUV n lateral motion model was: 



¥ 


Yaw angle 

X = 

V 

= 

Lateral (crosstrack) velocity 


r 


Yaw rate 


In a closed loop system the system output is fed back to the input. For example, 
the simplest form of feedback is u=-kx, where k is a gain vector of size (Ixn). 
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Substituting u=-kx into Equation (3.1) yields a general dosed loop state space 
system representation 

x«[A-Bk]x 

Many different forms of feedback can be used, as will be shown later in this 
chapter. 

B. STEADY STATE PERFORMANCE 

When a system is in steady state, the derivatives of state variables assodated 
with velodties equal zero. This makes the system analysis simpler. Analysis of the 
AUV n lateral motion began by assuming steady state conditions (e.g., and 
equal zero). During initial testing the AUV n rudders operated together, i.e., they 
both moved the same amount, though in opposite directions (5rt,=-S,^. 

The linear, simplified equations of motion (Equations (2.3) and (2.4)) can be 
solved simultaneously to produce an explidt steady-state expression for the yaw 
(turning) rate, r, and approximate turning radius, R. 

Y Pl’u(N^-N«)8 

r =---^^--- 

Y,(mxG - £pN,) - N,l(m - £l "Y,) 

2 . 2 

r = £ 


(3^) 





An exact expression for the ttiming radius R is R=1 -Equation (3.2) 

assumes that the lateral velocity, v, is small when compared with the forward 
velocity. 

During the AUV H's "maiden voyage," the propellers turned at a speed 
corresponding to a forward velocity of two feet per second. The rudders were 
manually set at the maximum values (23“). The AUV n was observed to turn at 
a rate of approximately 9“/min in a tiirrung radius of two vehicle lengths. These 
values were used as the baseline for determining the accuracy with which 
Equation (3.2) predicted the AUV n motion. 

Table VI shows the Estimate 1 steady state hydrodynamic coefficients and 
the corresponding turning rate and turning radius. The Estimate 1 hydrodynamic 
coefficients did not produce a sufficiently fast turning rate. The effect of each 
hydrodynamic coefficient on the turning rate was analyzed to determine which 
coefficients should be modified, and by what amouiit. 

The sensitivity of the turning rate to a change in a hydrodynamic coefficient 


is given by the slope of a curve of the turning rate versus the coefficient in 


question. Expressed mathematically, this equals __ where HC is the 

dHC 

hydrodynamic coefficient in question. Treating turning rate as a function of Y , 


Y^, N^, and N^, the turning rate sensitivities are: 



TABLE VI. STEADY-STATE, SURFACED AUV II PERFORMANCE 


1 

Estimate 1 

Estimate 2 | 

1 

-0.03896 

-0.03896 

-0.03896 

Y, 

0.01187 

0.01187 

0.01187 

N. 

-0.00714 

-0.00769 

-0.00769 

N, 

-0.01196 

-0.01022 

-0.01022 

Yerv Ysrt, 

0.02345 

0.02345 

0.02345 

u (ft/sec) 

2.0 

2.0 

1.5 

6 (degrees) 

23.0 

23.0 

23.0 

r (deg/sec) 

6.39 

8.79 

6.6 

R (vehicle lengths) 

2.45 

1.79 

1-79 1 


. (-^P(N5„-N,rt,)u6)DEN-NUM(£PN,-inxG) 
dr 2 ___2__ 

(Y,(inxG-£l"N,)-N,l(m-£PY;)2 

m m 

-num£i"n, 

dr ^ _ 2 _ 

(Y,(mxG-£PN,)-N,l(m-£PY,))2 
2 2 

-NUM(£13Y-m)l 

dr ^_ 2 _ 

(Y,(mxG - £l - N,l(m - £l ^Y,))^ 

2 2 

num£i«y, 

dr ^ _ 2 _ 

(Y,(mxG - £l "N,) - N,l(m - £l ’Y,))* 

2 2 



I 
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where 


NUM-£l»(N».-N»t)Y.u8 

iS 

DEN » Y.Cmxo - £l ^N,) - N,l(m - £l ®Y,) 

2 2 

These equations are cumbersome to work with. Directly plotting tiie predicted 
turning rate against different values of the hydrod^mamic coefficients provided 
an easier method by which to determine which coefficients most affected the 
turning rate. Figure 3.1 shows a plot of turning rate versus the hydrodynamic 
coefndents as they were individually varied from 50% to 150% of their Estimate 
1 value. 

The hydrodynamic coefficients associated with the moment of the vehicle 
due to yaw and lateral velocity, and N^, had the largest affect on turning rate. 
For this reason, it was decided to adjust the values of and N^, to values listed 
as Estimate 2 in Table VI. 

The results obtained using the Estimate 2 values were deemed close enough 
to use for additional modeling for various reasons. Since exact values of yaw rate 
and turning radius were not known, it was unreasonable to obtain "exact" 
matching with the observed values. It should be noted that the AUV n did slow 
down during turns. Simulations using an average forward velocity of 1.5 
feet/second also yielded results reasonably close to observed values. 
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0.S 0.8 0.7 0.8 0.8 i J t.l 1.2 1.3 1.4 1.8 


RELATIVE COEF *CIENT VALUE 

Figure 3.1 AUV II Turn Radius Sensitivity 

C AUV II LATERAL MOTION REPRESENTATION 

The general form of the state space equations describing the AUV II lateral 
motion are 

\jf a r 

V = ai,uv + a,2ur + b,iU + bjjU *5^ (3.3) 

r = ajjuv + ajaUr + baiU + baaU *5* 
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or, in matrix notation 


V 

c 

0 0 l' 
0 a,j a^j 

V 

V 


’ C4 

O 

r. 


0 aji ajj 


1 

^21 ^22, 


.“’V 


(8.4) 


The coefficients in Equations (3.3) and (3.4) were determined by simultaneously 


solving the linear, simplified equations of motion (Equations (2.3) and (2.4)) for 
V and i , and are listed below. 


a 


n 


a2i 






A4B2 - AjB^ 

&.A * 

A3B2 “ A2B3 

AjBj - AjBj 

^12 

AiB2 “ AjBj 

A^Bj - AjB4 

SUa * 

• " 

A3BJ - A]B3 

AjBj - AjBj 

«*22 

.A^Bi-AjBj 

A5B2 - AjBg 

b„ = 

A(B2 - A^Bg 

AjB2 - AjBj 

*^12 

A1B2 - AjBj^ 

A5B, 

b„ = 

A3B1 “ AjBg 

.A2B1 - AjBj^ 

“22 

AjBj - AjBj^ 


where, expressing the hydrodynamic coefficients in non-dimensional form. 


A, = m - 

A2 = mxG -Y^ 

Aa-Y,-m 

A,=Y, 

A5=Y«„ 

A«=Y^ 


B, = mxc ~ 

Bj * Iz-N, 

B3 = N,-mxG 


B6 = -N. 


Srb 
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In this form, the open loop dynamic matrix is singular, and one pole will always 
be located at zero. An alternative form of deHning the system would only indude 
the rows associated with lateral velodty and yaw rate. In addition, the system as 
dehned contains two inputs, the bow and stem rudder angles. During initial 
operation of the AUV n, the rudders were not operated independently. Thus, 
Equation (3.4) simplifies to 


r 1 '■ in fk 1 

y ®u *12 V ,« 

■ u 0 

/J Sj, ajj jrJ bj^ 


(3^) 


where = 6 

Sh, = -8 

b, = -Kb,, - b,j 

b2 * ” b22 


D, CLOSED LOOP SYSTEM CONTROL DEVELOPMENT 

1. Transfer FtmcHon Fonnulation 

A marine vessel is steered to a required heading (y) by using the 
rudders. For this reason, the relationship, or transfer function, between y and 5 
is of primary concern when developing a control law. The solid lines in Figure 3.2 
show the block diagram representing the AUV n steering plant as expressed by 
Equation (3.5). 



3PI 




Figure 3.2 Simple Proportional/Derivative Controller 


Simultaneously solving the above equations yields the transfer function 
between y and 5 

- (a„aj 2 )uy (a„aj 2 - aijaj,)u 

• bju’S ♦ (a2,bj - ai,b2)u’5 

Expressed in the "s," or Laplace domain, the transfer function becomes 

y^ b2U*g^(a2ib,-a„b2)u» 

6 s[ 6 V-(a„ •►a 22 )us+ ( 8 , 1822 -a,ja 2 ,)u*J 

Up to this point only the oj>eh loop situation has been addressed, i.e. 
provide a given rudder angle, 5, and observe the change in heading, y. In 
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practice, the AUV II must maintain an ordered heading, y,. This requires the use 
of a closed loop controller. 

A simple proportional/derivative controller of the form 

5 ■ K,y, ♦ Kjy, where y, ■ Vo “ y ^^7) 

was chosen for the initial form of the AUV II lateral motibn controller. This form 
of controller is easy to implement in a situation such as this where the rate of 
change of heading (and thus rate of change of heading error) b able to be 
determined explicitly from the output of the "plant.” It also has the added 
advantage of providing a more rapid and better damped system response than 
a controller that uses only the actual value of the heading error. 

Since y=r. Equation (3.6) can be integrated to obtain a transfer function 
strictly between r and 5 


r 

^ s^-^djSt-dg 


(3J) 


where no = (a^, b, - a,, bj) u^ 

n, = bj u^ 

do — (ajj 322 “ ^12 ®2I ) 

d, = -(a„ + 322) u 

By combining Equation (3.8) with the control law of Equation (3.7), an expression 
for the transfer function between y^ and y can be determined 


K,(n,s^n„) 

y„ * (K, ♦K,s)(n,s^n„)-s(s^^d,s + d„) 
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The denominator of Equation (3.9) is the characteristic equation of the AUV n 
lateral motion performance model subjected to a step input Through analysis 

techniques such as an s-plane pole and zero plot, the desired response of the 
AUV II can be established. 

2. Second Order System Response 

The AUV n lateral motion system as represented by Equation (3.8) is 


a second order system. In general, a second order system 

can be represented is 

the s-domain by 

• 


■ 

C(s)= R(s) 

(3.10) 


(s* + 2;to„^(D^) 

' 

where C(s) = 

output(v) 

, 

R(s) 

input(Vo) 



system natural frequency 



damping ratio 



When the input,, R(s), is a unit step input, such as a normalized ordered heading 
(Vo), Equation (3.10) becomes 

C(s) =-—-— (3.11) 

8(s^ + 2;a)o+ ©*) 

for which the transient output is 

c(t) = v(t) = 1 -e ■^“•‘sinfcOoPt + 0) 

P 

where p - ^ and 0 = tan'’(P/Q. 
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The transient output of the system is defined by the swiftness of 
response as measured by rise time (T,) and time to peak value and the 
closeness of response to the desired peak value (Mp) and settling time (T^. When 
analyzing the response of system to a step input, the most common parametere 
used are settling time and percent overshoot (P.O.) which is related to rise time 
and time to peak value. The settling time is the length of time required for the 
system response to stabilize within a certain p)ercentage, usually 2%, of the final 
system value. For a second order system with a damping constant of sco^ the 
response will remain within 2% after four time constants: 


T. 




Percent overshoot, using the ordered course, as the input, is defined by 


M - V 

P.O. = -.P I.°yl00% 

Vo 

The response of a second order system dep>ends strongly upon the value of the 
damping ratio (Q. If ^ is low, the system will respond more rapidly (decreasing 
T, and Tp), but oscillate more around the final value, increasing peak value of the 
output as well as the settling time. A system with a large value of ^ will respond 
slower (longer T, and Tp), but it won't oscillate as much. 

3. Gain Determination 

Figure 3.2 can be simplified into the "reduced" form of Figure 3.3 in 
which the entire AUV II "plant" is represented as one block. 
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Figure 3.3 "Reduced** Representation of Proportional/Derivative 
Controller 

The transfer function between the ordered course (\j/„) and the actual 
heading (xj^) is given by Equation (3.9). The denominator of Equation (3.9) forms 
the characteristic equation 

+(di-K2ni)s*+(do-K2no-Kini)8-Kin<j =0 (3.12) 

which can be analyzed using root locus techniques. First, Equation (3.12) is 
rearranged to separate K, 

1+K,-(3.13) 

s ^ + (dj-K 2 *'i)s * + (do-K 2 no)s 

Equation (3.13) has zeros at no/ni=0.2183. One pole is located at s=p. The other 
two poles are obtained by solving the denominator of Equation (3.13) 

s*-^(d,-K2nj)8^-(do-K2no)=0 (3.14) 
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Note that Equation (3.14) is strictly a function of Kj. An iterative method was 
used to determine optimum values of K, and Kj, in which values for Kj were first 
chosen, and corresponding values of K, were then obtained graphically using an 
s-domain plot. 

A damping ratio {Q of 0.707 was chosen as an acceptable goal. For a 
step input (e.g. applied rudder angle) a damping ratio of 0.707 provides a rise 
time of four time constants with approximately 5% overshoot. On the s-domain 
plot, a line that bisects the angle between the real and imaginary axes (45“ from 
the vertical) represents a damping ratio of 0.707. 

Figures 3.4 and 3.5 show the s-domain plote for choices of K2=0.5 and 
2.5 respectively. The "Xs " on the real axes on both figures represent the locations 
of the system poles for Kj=0. By increasing Kj from zero, the location of the two 
system poles of Equation (3.14) was changed. For K2=0.5, a damping ratio of 0.707 
was obtained for values of Ki=0.5 and 2.5. For K2=2.5, a value of K,=6.5 produced 
a damping ratio of 0.707, and K,=2.95 produced a damping ratio close to 0.707. 
The simulated performance of the AUVII to a course change of 10° was analyzed 
for each of the four combination of gains obtained through s-domain analysis. 
Figure 3.6 is a plot of rudder angle (5) versus time, and Figure 3.7 shows heading 
angle (v) versus time. Time on both these plots is normalized; one unit is the time 
for the simulated AUV n to travel one shiplength. Figure 3.8 is an X-Y 
(geographic position) plot of the simulated AUV II through the turn. Table VH 
summarizes the performance of the four different gain combinations. 
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NORMAUZEDTIME 

Figure 3.6 Rudder Angle vs. Time for 10“ Turn 



Figure 3.7 Heading versus Time for 10“ Turn 







Figiire 3.8 Simulated AUV II Position Dining 10“ Turn 


TABLE VII. SIMULATED AUV II PERFORMANCE 
FOR VARIOUS GAIN COMBINATIONS 


Option 

Number 

Value of K, 

&K 2 

Normalized 

Rise Time 
(Vehicle Lengths) 

Course 
% overshoot 

K, i 

K, 

1 

2.5 ! 

0.5 

2.8 

11 

2 

0.5 i 

0.5 

>10 

N.A. 

3 

6.0 i 

2.5 

2.5 

8 1 


2.95 i 

- 1 - 

2.5 

4.1 

4 1 


Analysis of the Figure 3.6 shows that the best rudder performance was 
obtained using gain options 1 and 4. The full capability of the rudder was not 
used in gain option 2, and an excessive amount of reverse rudder was used in 
gain option 3. Figure 3.7 shows the least amount of course overshoot was 


40 


/ 



































obtained with gain option 3, with gain option 1 providing the next best result. 
Gain option 4 had almost no overshoot, but the tine to achieve final course (rise 
time) was much longer than for either gain option 1 or 3. Gain options 1 and 3 
provided the tightest track, though the track obtained with gain option 4 is 
acceptable. Yielding from the gain option 1 resulted in the best overall 
performance of the simulated AUV 11. 

The best overall performance was obtained with gain options 1 and 4. 
Gain option 1 was chosen as the one to use in the actual AUV II for initial testing 
because the actual values of K, and were less than for gain option 4. This 
translates to less rudder activity and power consumption on the AUV II, an 
important consideration due to the limited battery life. 

E. COMPARISON WITH ACTUAL AUV II PERFORMANCE 

Initial closed loop control, in-water testing of the AUV II was performed in 
the Naval Postgraduate School swimming pool. This environment was free of 
outside disturbances, such as currents and high winds. A racetrack pattern was 
used for initial closed loop testing. Table VIII summarizes the heading commands 
to the Al/V II control system, using a racetrack pattern. 
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TABLE VIII. INITIAL AUV II TESTING COURSE COMMANDS 


Time (seconds) 

Course (degrees) 

0 

000 

30 

,80 

90 

360 1 


Figure 3.9 shows a plot of the AUV 11 heading versus time using data 
obtained from the onboard heading gyro. Superimposed is the simulated AUV n 

I / 

heading for the same racetrack pattern. The constant offset in heading is caused 
by model inaccuracies and the speed difference between the simulated AUV H, 
which was assumed to be a constant 1.6 feet per second, and the actual AUV II, 
which increased In speed from 0 to approximately 1.6 feet per second during the 
first 30 seconds. If th.r fivst .30 seconds of data are discarded, the heading match 
between the simulaS d and actual AUV II's is extremely close (Figure 3.10). 

Figure 3.11 is i: plot of simulated and actual AUV II turning rate versus 
time. There is close agreement between both results, though the actual AUV II 
turning rate increased faster than the computer model. Figure 3.12 shows the 
rudder pjerformance. 

The initial pierformance of the AUV II was very encouraging. Hydrodynamic 
coefficients predicted using primarily steady state turning analysis and linearized 
equations of motion produced good results, noteworthy considering that when 
turning radii as small as the AUV II's occur (<4 ship lengths) significant non- 
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linearities are encountered in ship modeling. The correlation between actual and 
simulated results was sufficiently close not to require additional modifications 
prior to submerged testing of the AUV 11. 

Information still lacking at this point in ti»e .e'sfing program was the accurate 
measurement of the AUV II speed. Additional analysis and refinement of the 
AUV II computer model had to wait until the onboard speed sensor was 
calibrated. Chapter FV will describe submerged AUV U testing, final 
determination of the lateral motion hydrodynamic coefficients, and the 


determination of other important hydrodynamic coefficients for the six-degree-of- 


freedom computer model. 
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IV. AUV JI SIX'DEGREE'OF'FREEDOM COMPUTER MODEL 


A. EQUATIONS OF MOTION 

Further devddpment of the AUV n model required the use of a complete 
three- dimensional, six-degree-of-freedom computer simulation. The six equations 
of motion for a submerged vehicle [Ref. 101 (surge, sway, heave, yaw, pitch, and 
roll) were incorporated into the computer code without using any simplifying 
assumptions. This permitted the maximum flexibility in determining the level of 
model sophistication. By setting various hydrodynamic coefficients to zero, 
simpler models could be analyzed. Integrations used to calculate the drag forces 
in the lateral, heave, pitch, and yaw equations are perfonned numerically using 
the tTap>ezoidal rule. 

The computer program has the capability to simulate a submerged vehicle 
using all hydrodynamic coefficients. In developing the AUV II model, a number 
of simplifying assumptions were made. These assumptions and known physical 
characteristics of the AUV n are summarized below: 

1. The AUV n is neutrally buoyant: W=B 

Z The AUV n is symmetrically loaded in the transverse direction 
(yc=0 and yB=0), and the vertical center of buoyancy is midway 
betw’een the top and bottom of AUV n (Zb=0). 

3. The counter-rotating propellers produce no yaw moment (Nj„p=0). 
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4. The products of inertia about the body system zero because the 
AUV n possesses two axes of symmet^. 

5. The AUV n acceleration and decelerations rates are small enough 
so that propeller slip can be neglected. 

6. The effect of aoss-coupled hydrodynamic coefficients can be 
neglected in most cases, again because of the AUV II geometric 
symmetry. 

The resulting equations of motion are presented below. 

mW “ mx^q - Z^q - « 

muq - mvp - mx^pr + mz^fp * q ’) 

+ Z,uq ^ Z^uw * u *(Zj. 5 . -KZjbSb) (4.1) 

- [Cp^(x)(v-*-xr)^ ->-Cp,b(xXw-xq)^. ^y^l^? , j .dx 

Urf(x) 

Heave Equation of Motion 

mil + mz^q -X^ii » 

mvr-mwq+ mxGq*+mxcr®-mzgpr+ + (42) 

. u 

Surge Equation of Motion 
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where 


mv + mx^r - mz^j) - Y/ - » 

mwp - mur -mx^pq 
- mz^qr Y,ur + Y^uv + u ^*^8^+Yj_8„) 

- [CD^(xXv-^xr)»^CD.b(xXw-xqn.^;i!E2dx 

UJx) 


Sway Equation of Motion 


(4^) 


"T' 


I,^>-mzGV-K^ = 

(ly -I,)qr + mz^ur - mz^wp + K^up - (ZqW - ZgB)cos6sin<{> 

RoU Equation of Motion 


(4.4) 


lyq - mxGW + mzcU - M^q - M^w = 
(I, - I,)pr - mXfjuq + mx^vp + mzcvr - mz^wq + 
MqUq + M^uw + u *(Mj,8,+- (z^yW -ZBB)8ine 

- r*~ [Ci,^(x)(v+xr)2+CD,b(x)(w-xq)2]i^lI^3lxdx 

•*- Uj/x) 

Pitch Equation of Motion 


(4.5) 


IjT + mx^jV - NyT - N^v = 

(I, - Iy)pq - mxcur + mxjjwp + N,ur + N^uv 
u ^(N5^8rt,+Nj^8„) + (XqW -XBB)cosesin<l) + u 

- [*■” [Cp^CxXv -t-xr)^ Cp^bCxXw -xq)^ x dx 


Yaw Equation of Motion 


Uc<(x) = [(v+xr)^ + (w - 


(4.6) 
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In addition to these equations, the six-degree-of-freedom computer model 
includes equations for the euler angle rates (\j/, (ft, 6) and inertial position rates 
(x, y, ±). These equations are contained in [Ref. 10] and can be easily interp/rted 
from the six-degree-of-freedom computer model in Appendix B. 

B. ESTIMATION OF HYDRODYNAMIC COEFnaENTS 

The determination of the hydrodynamic coefficients in the yaw and sway 
equations (4.3 and 4.6) was discussed in Chapters 11 and III. This section will 
discuss the determination of the hydrodynamic coefficients in the remaining 
equations of motion. 

1: Heave and Pitch Equations 

The hydrodynamic coefficients in the heave equation (4.1) Z^, 
and Z^, and pitch equation (4.5) M^, M^, were determined by 

geometrically scaling the given SDV hydrodynamic coefficients. The 
hydrodynamic coefficients related to the accelerations are a function of the added 
mass of the vehicle. Due to the similar, and fairly rectangular shapes of the AUV 
II and SDV, the coefficients can be considered proportional to the enclosed 
volume, or mass since both vehicles are neutrally buoyant. For example 

(^sDv ~ )sDv “ 

where Z is the dimensionless hydrodynamic coefficient. (For convenience the 
prime has been left off in other discussions regarding hydrodynamic coefficients. 
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and will again be left off after this section). Writing a similar expression for 
(ZJ Auv/ and taking the ratio between the two coefficients yields 


N3 


(Z:)Auv=(Z:)gDV 


-•SDV 


^Lauv^ 


m 


AUV 


nSsDv 


By substituting in the appropriate vehicle dimensions and masses 


(Z^ )auv ® 0.3718 (Z* )sDv 

This same ratio applies to the geometric scaling for the other acceleration-related 
terms. 


The velocity-related hydrodynamic coefficients are related to drag forces 
on the vehicle. For the pitch and heave equations the areas of interest are the 
vehicle top and bottom. For example 


(ZJ; 


'SDV 




'SDV " ^ ^SDV ®SDV 


Again taking a ratio between AUV II and SDV hydrodynamic coefficients yields 


(z:)auv=(z:); 


SDV 


\f 


■'SDV 


■•AUV 


B 


AUV 


B 


SDV 


= 0.5195(Z^)auv 


Table IX summarizes the results of geometrically scaling the hydrodynamic 
coefficients for the pitch and heave equations. 
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TABLE IX. PITCH AND HEAVE EQUATION 
HYDRODYNAMIC COEITICIENTS 


1 Coefficient 

SDV Value 

Scaling Factor 

AUV n Value 

Z 

W 

-0.30200 

0.5195 

-0.15687 

z 

s 

-0.13500 

0.5195 

-0.07013 

M. 

0.09860 

0.5195 

0.05122 

M, 

-0,06860 

0.5195 

-0.03563. 

Zw 

-0.24300 

0.3718 

-0.09340 

2, 

-0.00681 

0.3718 

-0.00253 

M 

w 

-0.00681 

0.3718 

-0.00253 1 


-0.01680 

0.3718 

-0.00625 


2. Roll Equation 

The primary hydrodynamic coefficients, and K^, in the roll equation 
(4.4) were also determined by scaling the given SDV hydrodynamic coefficients. 
When a vehicle enters a turn the amount and direction of roll is a function of the 
location of Cg and Cq and the lateral force caused by lateral motion (Y^). Prior to 
scaling, the effect of the fin on the SDV had to be "removed" from the given 
values of and K^. This was done by multiplying the given values of andK 
by the ratio of the given SDV and Finless SDV values of Y^: 
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. -0.01100 

f -0.03585 

(Y ) 

^ ' * V'gIVTO ^ 

f > 

[-0.05550 



= -0.00101 

f -0.03585 


-0.05550 


-0.00711 


= -0.00065 


As in the case of the heave and pitch equations, the acceleration coefficient is a 
function of the added mass term, and is proportional to the masses of the 
vehicles. Thus 

(K W = 0.3718(Xp' )sDv = (0.3718)(-0.0065) = -0.00024 
The velocity-related coefficient, K^, is a function of the area of the vehicle side, 
normal to the lateral velocity vector. 


(^)sDV “ ^SDV “ KTgDv ^ 


SDV 


and taking the ratio betv^een AUV n and SDV terms yields 


^AUV ” 


SDV 


v 


■“sov 




■^AUV 

v^Tsdv^ 


= (-0.00711)(0.7598) = -0.00540 


3. Surge Equation 

The acceleration-related hydrodynamic ojefficient, X^, in the surge 
equation (4.2), was estimated by geometric similarity in the same way as 
described for other acceleration-related hydrodynamic coefficients. 
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OC)auv • 0.3718(X/)8dv * -0.00282 
In order to account for nonlinearities in the equations of motion that 
become significant when turning radii as tight as the AUVII has are encountered, 
two cross-correlation coefficients were included in the AUV n model. the 
hydrodynamic force in the x direction as a function of yaw rate, and X^, the 
hydrodynamic force in the x direction as a function of lateral velocity were 
estimated for the given SDV values by geometric similarity. The area on the 
vehicle of concern for these coefficients is the side, thus both X^ and X^ are 
proportional to the product of vehicle length and draft. 


)sov “ KLsovTg 
* ■^L*(X„)s0v “ KLguvTg 


Taking ratios of expressions for X^ and X„ for the AUV n and SDV yields 


(X4)auv = (X4)sdv ^ =(0.05290)(0.7598) = 0.0401 


^AUV It 


(X4)auv = (XX)sdv ^ =(-0.00401)(4.327) = 0.01735 


^AUV I [ "^SDV 


Neglectin g propeller slip, Xj^^p is proportional to the overall vehicle drag 

coefficient. Coo- The process begins with the general equation of propulsion force 
u 

where 11 =—I, u^ = speed (ft/sec), and n^ = rpm. Rearranging terms, and 
n 

o 

collecting coefficients results in the following non-linear differential equation: 
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(4.7) 


(m-£l%')u>£l'u*X;^ 

2 m 

.£pC„(Tin)^-£PCDou' 

2 2 

(m-X^)u+au* = where a = £l*CDo 

2 

This equation can be solved by isolating time and speed-related terms on opposite 
sides of the equation sign, and then integrating from an initial condition of uasO 
at t=0 to u=Uo at t=t. The resulting expression for speed, u, is 

u = Tin.^_li., where A = (4,8) 

e^‘ 4.1 m-X^ 

To find a value for Cdo an expression for a in Equation (4.7) in terms of the other 
coefficients is required. By assuming u to be a linear fraction (a) of u<^ when t=T, 
and n=no, then 

e*’’-! 
au„=nn.i- 

e T1 

Inserting the expression for A from Equation (4.8), and after some algebra, an 
equation for a is obtained 

a = .^Ll2^1nfll^ll (4.9) 

2u„T [l-a^ 

Placing Equation (4.9) into the definition of a in Equation (4.7), and rearranging 
terms yields an explicit expression for Cpo 
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(4.10) 


m-|l% 

Cdo =- 


1 + a 


1 -a 


nJpV 

Observations of the AUV n during its maiden voyage indicated the vehicle 
achieved full speed after a bit more than 20 seconds. By assuming that the AUV 
n had achieved 90 percent of maximum speed at 20 seconds, using Equation 
(4.10), an estimated value for Cd© of 0.015 was obtained. 


C ACT UAL AUV II PERFORMANCE COMPARISON WITH MODEL 

1. Speed 

A computer program was written to compute the AUV n speed versus 
time for various values of Cd©- Figure 4.1 shows a plot of speed versus time for 
three different values of Co©- Included on the figure is a plot of actual AUV n 
speed during initial acceleration. The two second offset from zero is due to a data 
recording problem in the AUV H; shifting the curve to the left shows that it 
coincides very close to the Cd©= 0.015 curve. Figure 4.2 is a plot of time to reach 
a certain percentage of top speed versus C©©. Curves are plotted for 90%, 95%, 
and 99% of maximum speed. The AUV n reaches 90% of top speed in 
approximately 12 seconds. 

2. Run Profile 

The following figures show how dose the AUV n model matches the 
actual AUV II performance. A "Figure 8" run profile was used for this 
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TIME (Seconds) 


TIME (Seconds) 


Figure 4.1 AUV !I Actual Speed Versus Simulated for Different Values 
of Cjxj 


o 



Figure 4.2 


Time to Achieve Percentage of Final Speed 





comparison. Ordered AUVII spaced was two feet/second, and the ordered depth 
was two feet. 

Figure 4.3 shows the rudder commands used to drive the AUV II 
through the run. After an initial straight run, the ordered rear rudder angles were 
±15°; ordered bow rudder angles were the opposite of the stem rudder angles. 

Figure 4.4 compares measured AUV II turning rate with the computer model. The 
actual AUV II propeller speeds are shown in Figure 4.5. Note that the rpms are 
different; the right motor saturates at a lower rpm than the left motor. Fifteen 
seconds into the run, the speed controller on board the AUV II ordered lower 

rpms to maintain a speed of two feel per second. Shortly thereafter the first turn ,; 

i 

began which caused speed to drop, and full speed was ordered on the propellers 
for the remainder of the run. The gradual drop in rpm during the remainder of 
the run was due to a drop in battery voltage. 

Figure 4.6 compares the computer model and actual AUV II spjeeds. ' 

There is close agreement between the model and actual speeds. 

The AUV II model depth performance is shown in Figure 4.7. Figure 4.8 
is a plot of the actual stern plane deflection versus time. The actual AUV II planes , 

moved quite a bit to maintain the vehicle on depth through the turns. 

Comparisons between actual and simulated vehicle pitch rate were not quite as 
satisfactory as for lateral motion. This is due to a lack of calibration of the 
hydrodynamic coefficients in the vertical plane as was done for the lateral 
equations of motion. Nevertheless, the developed computer model provided an 

y 

/ 

/ 
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TURNING RATE (Degreec/Seoond) g RUDDER ANGLE (Degraes) 


O 



' O. O so. 0 100. O 150. 0 200. O 

TIME (Seconds) 


' 4.3 Rudder Commands for Figure Eight 



TIME (Seconds) 

Figure 4.4 AUV II Actual Versus Simulated Turning Rate 



58 




SPEED 


i 




T1M€ (Seconds) 


Figure 4.5 


Actual AUV II Propeller Speeds 




Figure 4.6 Actual Versus Simulated AUV II Speed 
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STERN 



Figure 4.7 Actual AUV II Depth 



TIME (Seconds) 
Figure 4.8 Actual AUV II Stem Plane Angle 
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excellent basis for designing an accurate depth controller, as evidenced by the 
results of Figure 4.7. 
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V. POSITION ESTIMATION AND LATERAL VELOQTY 
OBSERVER DESIGN AND CALIBRATION 

A. BACKGROUND 

The safe operation of a ship and its ability to perform assigned missions 
requires a continuous knowledge of position. Navigation equipment required in 
ships has been established by international convention [Ref. 26] and 
includes a marine radar system, radio direction finder, gyroscopes, and echo 
sounders. Additional equipments used to fix a ship's position include doppler 
sonar, satellite positioning receiver, LORAN and Omega. 

During the period between fixes a ship's position is estimated using a 
procedure known as "dead reckoning," in which the ship's position is projected 
ahead based on ordered course and speed. Ocean currents and errors in 
estimating or maintaining the ship's course and speed result in the generation of 
a circle of position uncertainty around the "dead reckoned" position estimate. 
Naval vessels commonly have an inertial navigator system to accurately estimate 
position between fixes. The inertial navigator consists of accelerometers mounted 
such that accelerations in sway, heave and surge are accurately sensed. The 
accelerations are integrated to determine the vessel's velocity along the three 
principle axes. 'i*he velocities are integrated again to obtain the distance the ship 
has traveled since the last fix. 
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Precise knowledge of the AUV n position is just as critical a problem as with 
any vessel. In fact, the absence of human intervention during a mission requires 
that navigation and motion control systems provide an extremely accurate 
estimate of position at adl times. The current configuration of the AUV n does not 
include a systeiTi for determining a navigational fix. 

The size of the AUV II limits the size and complexity of onboard navigation 
systems. Withirl the confines of the NFS swimming pool it is feasible to use the 
sona^- system for position fixing provided accimate sonar ranges to U.*.- walls of the 
swimming pool are available, that information can be used to determine the AUV 
n position. However, the quality of sonar returns from the walls of the swimming 
pool are frequently not accurate and consistent enough, esp)ecially during turns, 
to provide continuous and reliable fix information. 

The accelerometers on board the AUV n could also be employed as an 
inertial navigation system to provide a continuous and accurate estimate of the 
AUV II position. However, the sensitivity of the accelerometers, which must be 
capable of wathstanding gravitational acceleration, is insufficient to accurately 
measure typical AUV II accelerations which are in the range of .05g. 

Data available from instrunrientation on board the AUV n includes forward 
velocity (u), heading (y), and turning rate (r). Equation (5.1) shows the state space 
method to determine AUV n velocities in the x and y directions. 
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X * ucos^ - vsiny 
f » usin>|f + vcosy 


(5.1) 


Position is then determined by integrating x and y. Note that lateral velocity (v), 
is not available from instrumentation on board the AUV E. The "dead reckoner" 
on board the AUV E uses only velocity and heading information to determine 
position, i.e. v is always assumed to be 0. This results in significant position 
inaccuracies being built up during the AUV E operation. 

With these limitations in mind, three different methods were investigated 
to provide the AUV B with an accurate position estimator. The first method 
inv^tigated used the six-degreerof-freedom computer model developed in the 
previous chapter. A second method used a reduced order observer to estimate 
lateral velocity. The third method investigated was an explicit determination of 
lateral velocity based on the turning rate. 

B. SIMULATION POSITION ESTIMATE 

The six-degree-of-freedom simulation program was used to provide an 
estimate of lateral velocity to use in Equation (5.1). The program simulates the 
rates of change of lateral velocity (v) and turning rate (f) using Equation (3.5). 
After performing a first order integration to determine v 

V = V + (At)v (®‘2) 
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3 t and y are calculated using Equation (5.1). The present AUV n position is then 
determined by a first order integration of the form of Equation (5.2). 

The shortcoming of this method of estimating the AUV II position is that the 
actual turning rate, r.^,/ is not equal to the simulated turning rate, r^. Figure 5.1 
shows a comparison of r.^ and r,i„ for the first 100 seconds of the "Figure 8" nm 
profile analyzed in the previous chapter. The difference in the simulated r 
between Figures 4.4 and 5.1 is due to the fact that Figure 5.1 was obtained by 
using a, simulation of the horizontal plane equations only. Since no pitch motions 
and dive plane activity is present in this case, the model is, as expected, more 
responsive. Even if the AUV II hydrodynamic coefficients were exactly known, 
r,rt and would not always be equal. Over time this difference would result in 
an ever-increasing error in actual versus estimated positions. 

C REDUCED ORDER OBSERVER 

The development of a closed loop lateral motion control system for the AUV 
n as described in Chapter HI did not require a knowledge of lateral velocity. An 
accurate estimate of the AUV n position does require knowledge of lateral 
velocity. Because lateral velocity is not a state variable that can be measured, it 
must be estimated. 

A dynamic system in which state variables are estimated from known 
(measured) variables is called an observer. It can be shown [Ref. 27] that 
for an observable system, an observer can be designed such that the difference 
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Figure 5.1. Comparison of r^ versus r.j, 


between the state of the actual system and the state of the observer can be made 
to reach zero as fast as desired through pole placement techniques. If some of the 
dynamic system state variables can be measured, then a reduced order observer 
Ctm be developed to estimate the remaining state variables. 

The development of a reduced order observer for lateral velocity begins with 
the equations for f and v presented in Equation (3.3), and reproduced below. 

v = a„uv*ai2ur-^bj,u25„+b,2u*8rt, 

(6.3) 

r = a 2 ,uv + ajjur + bjjU * bjjU *8^ 
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In Equation (5.3) r is known, but v must be estimated by 


V « aijU^ + aijuCj y + bjjU ^5„ + b^u ^5^, 


By defining 


V » Ly + z 


( 5 . 4 ) 


where 


4 = Fz + Gy + Hu 


( 5 . 5 ) 


the estimation error can be defined as 


e = V - V = a, 2 ur + ajjuv+bijU^8,+bj2U^5b-Ly - z 

After a few algebraic steps, and grouping terms associated with r, v, 5b and 5^ the 
estimation error can be expressed as 

e = Fe - [ajjU - LCia 22 U + FLCj - GCjJr 
♦ [ajjU> LCjajjU - F]v 
^[b,,u2-LC,b2,u2-H,]5. 
-[bj2U*-LC,b22u2-H2]8b 


( 5 . 6 ) 


For the error to be indepjendent of r, v, 8b, and 8*, the matrices multiplying them 
must vanish (i.e. equal 0). Therefore, matrices F, G and H can be defined as 
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F » aijii - LC|a22U 

G = [HijU - LCja^jUlC '* + F 

H « [bi,u^ - LCjbjiU“ bijU * - LCjbjjU 

and, 

6 « Fe 

For the system to be stable, the eigenvalues of F must lie in the left half of the s- 
plane. Since F in this problem is a scalar, appropriate eigenvalues, equivalent to 
the observer time constants, can be directly determined without computing the 
determinant of a matrix. 

The observer gain matrix, L, also a scalar in this problem, is fptmd by 
solving 


F = SjiU - LajiU 



( 6 . 7 ) 


where represents the eigenvalue of F. The AUVII vehicle length is 1, and T„ 
is expressed in time to travel a certain number of vehicle lengths. Different values 
of the observer time constant, T^, result in different values of the observer gidn 
matrix. Once a value for L has been found. Equations (5.4) and (5.5) are used to 
calculate an estimated value for v which is then used in Equations (5.1) and (5.2) 
to calculate the AUV II velocities and the present AUV n position. 

Figure 5.2 is a plot of lateral velocity versus time for the first 100 seconds of 
the Rgure 8 run profile. Note the variation of estimated lateral velocity versus 
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time constant. Varying the time constant of a linear observer should only affect 
the speed with which the estimate converges to the exact value. However, in this 
instance varying the time constant also affected the steady state value of lateral 
velocity. The reasons for this will be discussed after the next section which 
describes the development of the "explicit" lateral velocity curve shown in Figure 
5.2. 



Figure 5.2 AUV II Lateral Velocity versus Time for Different Observer 
Time Constants 

D. EXPLICIT DETERMINATION OF LATERAL VELOQTY 

The expressions for v and f with the bow and stem rudders not operating 
independently are contained in Equation (3.5), and printed below in equation vic» 
matrix form. 
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V « a„uv + a,2ur + biU*8 (5,S) 

T » aijuv + ajjur + bjW *5 

I 

The rudder angle, 8 , can be eliminated by rearranging the equations, jdelding an 
equation that is a function of v, v, r and t only, 

bjV + (biaji “bjajiluv = bjr + (aijbj-bja2,)ur 

or, in the s domain 

KT3SV + Kv = K^r + K^T^sr 
where K = (bjaj,-bja^lu 

T 3 = bjAC (5-») 

= (aijbj - b,a 22 )u 

T, = b,/K, 

K and are constants and T 3 and T 4 can be treated as time constants of v and 
r respectively. 

Equation (5.9) can be rearranged to clearly show the explicit transfer 
function that exists between v cmd r 


v.K^d^T^s) 

7 ' Kd+TjS) 

from which an explicit solution for v can be derived 


( 5 . 10 ) 


1 K,T 4 . K, 

V = -_V* r* _ r 


KT, 


KT, 


( 5 . 11 ) 
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The "explicit" lateral velocity curve of Figure 5.2 was obtained by using Equation 
(5.10) to d‘tennine V. 

A possible problem exists with Equation (5.11) in that the derivative of r is 
used to calculate v. Any noise in the value of r is amplified by taking its 
deiivative, and this amplified noise has the potential to degrade the computed 
value of V. Therefore, it is desirable to eliminate the zero associated with t ■ This 
is accomplished by writing the reciprocal of Equation (5.10) and performing a 
Taylor series expansion of the right hand side. The resulting "first order" transfer 
function between v and r is 


Kv 


r K + K(T 3 -T,)s 

in which, in the time domain. Equation (5.12), v is a function of only v and r 


v - 


K 


( 5 . 12 ) 


T 3 -T, K(T 3 -T,) 

Figure 5.3 shows the explicitly determined lateral velocity as computed by the 
exact expression (Equation (5.11)) and the first order approximation (Equation 
(5.12)). Also shovm, for comparison, is the 0-th order approximation obtained 
from Equation (5.10) by substituting s=0. In tht cise of the AUV n the effect of 
noise in the value of f is minimal. Though tf.e amplitude of all the terms in 
Equation (5.11) are within the same order of maj^ raf ude, it appears that any noise 
introduced into the estimation of v is minimized by the subsequent integration 
to obtain v. 
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Figure 5.3 Explicitly Determined AUVII Lateral Velocity, Exact and First 
Order Approximations 

The expUcitly determined lateral velocity behaves as ah observer with a time 
constant between 1 and 2. This "natural time constant" can be computed directly 
from Equation (5.7), This time constant is a function of the AUV n hydrodynamic 
coefficients which make up the various coefficients of the a and b matrices. 
Substituting numbers into the equation yields a value of T^ of approximately 1.8. 
This is consistent with the location of the explicit curve on Figure 5.2. 

E. OBSERVER CHARACTERISTICS AND CALIBRATION 

The analysis so far has only compared different methods of estimating the 
AUV n lateral velocity. Still unknown is the actual AUV n lateral velocity, which 
is the benchmark against which the different estimation methods must be judged. 
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Since there is no way to determine the actual AUV n lateral veloJ*y, the six- 
degree-of-freedom computer simulation was used to determine ti.appropriate 
lateral velocity observer time constant. 

1. Six-Degree-of'Freedom Calibration of Lateral Velocity Observer 

Generating a track using the six-degree-of-freedom computer model of 
the AUV involved no estimates of sensor errors. Recognizing that the model is not 
yet a perfect representation of the AUV H, it could still be used to generate a 
track against which the performance of the enhanced position estimator could be 
evaluated. The inputs to the simulation were actual AUV n rpm and rudder 
angles as recorded during an oval nm prcfile performed by the AUV H. Actual 
rpm and rudder commands were fed into the simulation. The results of the 
simulation were treated as the truth against which the different lateral velocity 
observers were compared. Figure 5.4 is a plot of lateral velocity versus time. In 
addition to the real lateral velocity as determined by the six-degree-of-freedom 
computer model, results for observers with time constants of 1, 1.5, 2, and 2.5 
vehicle lengths are plotted. 

Analysis of Figure 5.4 reveals that the best time constant for the lateral 
velocity observer is approximately 2.0. The reason for the difference in the results 
for different observer time constants can be attributed to the non-linearities of the 
equations of motion used in the six-degree-of-freedom computer simulation. The 
lateral motion observers were developed based on linear, simplified equations of 
motion. Any non-linearities not accounted for in the observer design would result 
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Figure 5.4 Comparison of Simulated AUV II Lateral Velocity with 
Estimated Lateral Velocities 

in the error ii> estimating lateral velocity not being independent of r, t,v and v. 
Thus, the observer would "believe” the estimation error was zero, when it in fact 
was not. These "hidden" non-linearities are responsible for the variation in steady 
state lateral velocities seen in Figvues 5.2 and 5.4. 

In order to verify the variation in steady state lateral velocities during 
the turns was in fact caused by non-linearities and that the observer was working 
properly, the same speed and rudder commands from the oval run of the AUV 
n were used in a simulation program based solely on the simplified, linear 
equations of motion. Figure 5.5 is a plot of the difference between the simulated 
lateral velocity and the explicit and estimated lateral velocities. The differences are 
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almost insignificant, thus verifying the proper performance of the lateral velocity 
observer. , 



Figure 5.5 Difference Between Simulated and Estimated AUV II Lateral 
Velocities Using Linear, Simplified Equations of Motion 

Figure 5.6 shows the results of using the six-degree-of-freedom 
computer model using a version of program OBSERVE (Appendix C). The solid 
line is the actual simulated track, tlie "truth" which the enhanced position 
estimator had to match. The dashed line shows the position estimate that would 
have been calculated by the AUV n without lateral velocity information. The 
dotted line shows the estimated position of the AUV n using time constants of 
1.5, 2.0, and 2.5. It appears that the best time constant lies between 2.0 and 2.5. 
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The improvement in position estimation using the reduced order 
observer for lateral velocity is clearly seen. Figure 5.7 shows the difference 
between the simulated and estimated x and y positions for a time constant of 2.3. 
Nowhere is the error more than two feet. More importantly, since the position 
estimator is being driven by the lateral velocity observer, the cross track error is 
never greater than one foot. 

As pointed out before, the six-degreeof-freedom computer model of the 
AUV n is not yet |;er,rci. C.>nsequentiy, there will be some differences between 
an actual and simulated track. In particular, any error in lateral vdocity will be 
integrated into the estimate of position. As time progresses, this error will 
continually increase. 
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2. "Constant Pool Width" Verification 

It has been shown that the reduced order lateral velocity observer and 
the enhanced position estimator can accurately estimate the AUV II geographic 
position when using the six-degree-of-freedom computer model. The ability of the 
lateral velocity observer and enhanced position estimator to estimate actual AUV 
II position from an actual AUV II run provided a second, independent means of 
proving the validity of this technique to obtain a real-time position estimate of the 
AUV II. 

The actual track of the AUV II would have to be determined from sonar 
range data to the sides of the swimming pool. Fairly good sonar range data to the 
edges of the swimming pool were available in the data file from the oval run 
used in the previous section. In particular, this run contained two straight legs 
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over 25 seconds long which provided a good lateral position of the AUVII in the 
pool. The range data from the forward-looking sonar was also very gbod. Both 
of these ranges could be combined to provide track data on the long legs of the 
oval. 

Figure 5.8 shows the geometry and terms associated with the "constant 
pool width" method. Because the swimming pool is a constant width of 60 feet, 
the sum of the initial distance the AUV H was from the left edge of the swimming 
pool, the lateral distance traveled, and the range to the right side of the pool 
should be 60 feet, as described in Equation (5.13). 


Initial Range to Left Side of Pool 


Lateral Distance 


Traveled 


1 60 

i- Cauv 

Feet 

Range to Right 


Side of Pool 

1 



Figure 5,8 "Constant Pool Width" Geometry 


Initialraas«from Lataraldiatanoe ^ Banfatoriijit , (5.13) 

laftpoolwall traveled poolwall 
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Figure 5.9 shows the raw range data from the AUV n data file corrected 
for the actual vehicle heading angle. During the time period shown, the range 
shown is the range to the right wall of the swimming pool. The erroneous range 
spikes had to be deleted, or smoothed, prior to using the data file for further 
analysis. A routine was written to smooth the data that used a threshold of .3 feet 
of difference betw^n successive sonar returns. If the range changed by more than 
the threshold value the range was kept at the previous value until the actual 
range returned to a value within the threshold. The dotted line in Figure 5.9 
shows the areas in which the range information was smoothed. 



Seconds 
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The initial distance from the left wall was obtained by analyzing Figvire 
5.10, which is a graph of the first six seconds of the oval trial run. The average 
range appeared to be 6.1 feet. Even if the choice of average range was in error by 
0.1 feet, the effect on the overall constant width calculation was very sniall. 



Figure 5.11 shows the initial results Of the constant pool width 
calibration using a horizontal plane dead reckoning program with a time constant 
of 2.0. Tnough the horizontal dead reckoning program , uses only the lateral 
motion, its esponse is very close to the six-degree-of-freedom computer model. 
The curv'es m Figure 5.11 represent the width of the pool as calculated by 
Equation (5.13). Both range terms in Equation (5.13) are known, hence any errors 
are due to an incorrect estimation of lateral di5<tance traveled. 
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Figure 5.12 is a geographic plot of the AUV II oval track and range 
data. The solid line represents the position of the AUV n as dead-reckoned by the 
AUV n. The AUV II computes down and aoss track velocity using Equation (5.1), 
with v=0.0. The dashed line is the enltanced position estimate of the AUV 11 using 
a time constant of 2.0. The straight lines along each side of the oval tracks are the 
ranges to the side of the pool obtained by the AUV II during the run. 

A quick analysis of Figure 5.12 indicates that the enhanced position 
estimate did a much better job of determining the act’idi position of the AUV II. 
Closer observation of Figure 5.12 reveals some problems with the onboard sensors 
which affect the validity of the results presented. 
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Figure 5.12 Geographic Piot of AUV II Oval Run 



Both edges of the oval were supposed to be parallel to the pool sides. 
It can be seen that neither leg of the AUV II is parallel to the pool sides. This was 
caused by a drifting problem with the heading gyro. Initial pool side tests of the 
heading gyro drift indicated an approximate drift rate of 0.014 radians/sec. 
Subsequent testing indicated that drift rate varied as the supply voltage varied. 
The supply voltage varied depending up)on the rpm of the shafts. To a lesser 
degree, the turning rate gyro also drifted during this run. Both of these drift rates 
had to be approximated to produce Figure 5.12. 

It can also be seen that the AUV II track turns prior to the end of the 
straight range line. This is due to an inaccuracy in the calibration of the speed 
sensor on the AUV 11. The solid line in Figure 5.13 shows the range to the far 
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end of the pool during the initial leg of the oval. The dotted line during the first 
nine seconds is a smoothed range computed by integrating the recorded AUV D 
speed. The constantly decreasing range from 10-33 seconds indicates the AUV n 
speed was approximately 2.3 feet/sec, though the onboard data file reported 
sp)eed approximately 1.6 feet/sec. 



Figure 5.13 Range to Far End of Pool, 0-40 Seconds 

Nevertheless, compensation for the combination of sensor errors is 
difficult. The sensor errors notwithstanding. Figure 5.12 does show that the 
enhanced position estimator can be used to produce a better estimate, of the AUV 
II position. 

It is noteworthy that the best lateral velocity observer time constant for 
both the six-degree-of-freedom computer model and horizonal dead reckoning 
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methods was approximately 2.0. These independent methods of determining the 
performance of the lateral velocity observer and enhanced positibn estimator 
prov.' method is sound. Improvements to the estimation of position in the six- 
degi > or-' IT’’lation computer model will occur when the AUVII hydrodynamic 
coeffideacs ^e more accurately known. When the problems of sensor errors in the 
AUV n itself are solved, the accuracy of the estimated position will be much 
better. This research has demonstrated that the reduced order lateral velodty 
observer and enhanced position estimator can be used to Obtain a more accurate 
estimate of the AUV n position. 



VI. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 

This thesis describes various aspects of research directly related to 
supporting the initial phases of in-water testing of the NFS AUV H. For the first 
time in this project the hydrodynamic coefficients of the AUV n vehicle were 
estimated. Table X summarizes the hydrodynamic coefficients and significant 
geometric properties of the AUV n as determined by this research. 

A proportional-derivative controller was designed for initial closed loop 
testing of the AUV 11 in the NFS swimming pool. The performance of the vehicle 
was as predicted by computer simulations. Initial turning response of the vehicle 
proved to be very satisfactory. 

Labor, equipment, and time constraints limit the amount of in-water testing 
that can be p)erformed with the AUV II. A number of components within the 
AUV n have limited lifetimes, e.g., the gyros have a service life of only 200 hours. 
For these reasons, it is important to conduct computer simulations of the AUV n 
to test various giiidance and control schemes. To this end, a six-degree-of-freedom 
computer model of the AUV n was developed. 

Space limitations on board the AUV n restrict the an-iount of navigational 
equipment that can be installed. To provide the AUV n with an accurate means 
by which to estimate current position, a reduced otder observer for lateral 
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velcxdty was developed and validated. By using the estimated lateral velocity with 
' actual AUV n speed and heading it was demonstrated that the AUV n position 
can be accurately estimated. The enhanced position estimator was tested using 
both simulated and actual AUV n trial runs. 

B. RECOMMENDATIONS FOR FUTURE RESEARCH 

The hydrodynamic coefficients for the AUV n must be refined. Research 
being conducted by Bahrke [Ref. 28] in the estimation of hydrodynamic 
coefficients using parameter identification techniques should yield a more 
accurate set of hydrodynamic coefficients. The coefficients will then result in 
improved accuracy of the six-degree-of-freedom computer model and enhanced 
position estimator. 

The controllers and guidance schemes investigated for the AUV I and SDV 
Mark 9 vehicles should be tested with the AUV IJ. The six-degree-of-freedom 
computer model is ideally suited to ^rform this testing. 

The enhanced position estimator should be incorporated into the guidance 
methods, and research conducted as to the best method by which to generate 
heading commands based on the current estimated portion and next waypoint 
generated by the mission planning software. 

The spjeed measurement of the AUV n should be improved. As a minimum, 
the paddle wheel speed sensor should again be calibrated. The performance of the 
paddle wheel during a turn, where the actual vehicle velocity is not normal to the 
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paddles, needs to be evaluated. Perhaps an improved speed sensor, such as a 
pitot tube should be installed. 

The effect of improvements to the AUV n should be studied using the six- 
degree-of-freedom computer model. Topics of immediate concern include: 


1. The installation of thrusters. Thrusters will add additional tenns to the 
equations of motion; the reduced order observer and enhanced position 
estimator can be easily modified to account for the thruster effects. 

2. Separate control of bow and stem rudders. Analysis of the NACA 0015 
plane characteristics would indicate that the K ■' rudder might stall 
during the sharp turns conducted by the AUV D. Separate control of the 
rudders would prevent stalling, and might even in: prove the 
performance of the AUV 11 in a turn. 

3. Separate control of the shafts. Independent operation of the shafts will 
enhance maneuvering and position keeping at slow speeds 


With the incorporation of any of the above improvements to the motion 
control systems of the AUV II, the governing equations of motion become more 
complex. Fortunately with the design of the reduced order observer, the enhanced 
position estimator is quickly adaptable to an inaeased number of parameters. 
With an accurate set of hydrodynamic coefficients, the six-degree-of-freedom 
computer model and enhanced position estimator will significantly improve 
future AUV n performance. 




TABLE X SUMMARY OF HYDRODYNAMIC COEFHaENTS 
AND SIGNinCANT GEOMETRIC PROPERTIES 


Coefficient 

Value 

Coefficient 

Value 

x„ 

-0.01735 

H, 

-0.03565 


0.00282 

Mw 

0.05122 


-0.04019 

Ma. 

-0.337*L*Yfc. 

Xsrfs , 

0.02345 

M«> 

0.285*L‘YfcB 


0.02345 

N. 

-0.00047 

^t6t 

0.02345 

N. 

-0.00178 


0.015 

N, 

-0.01022 

X 

-0.00178 

Nv 

-0.00769 

Yv 

-0.03430 


-0.337*L*Ya„ 

Yr 

0.01187 

e 

0.283*L*Yjrt, 

Yv 

-0.03896 

!.(«*) 

2.7 

Y6„ 

0.02345 

ly(ft^) 

42.0 

Yert. 

0.02345 

I, (ft^) 

45.0 


-0.00253 

x„ (ft) 

-0.377*L 

Zw 

-0.09340 

XH,(ft) 

0.283*L 

z. 

-0.07013 

Weight (lbs) 

435 

Zw 

-0.15687 

Length (ft) 

7.3 

Zss 

-0.02345 

p (slugs/ft^) 

1.94 

Zgb 

-0.02345 

Xc (ft) 

0.0104 


-0.00024 

Zc(ft) 

.05 

Kp 

-0.00540 

Xb (ft) 

0.0104 

M, 

-0.00625 

^DY 

0.5 

M 

w 

-0.00253 

Cdz 

0.6 
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the final expression for Yr, is obtained: 







3C, 


Lisp 




Tor small angles of attack (Cq), is small in relation to 


(A3) 


9C 

_I: md can be ignored. 

spr 


By expressing (Yv), in normalized, nondimensional form equation A3 becomes 


(Yv)f 


i / 

f \ 

ac^ 

“ Af 

^ Jf 


(A4) 


Note that the prime in equation A4 indicates that the fin area has been 
normalized by dividing the actual area (ap) by l^„f. For the rest of this appendix 
only normalized dimensions will be used in equations, and, for simplification, the 
prime will be left deleted. 

The total area and centroid of the SDV fin was determined by graphical 
integration. Figure A1 shows the SDV rear fin, and the eight areas in to which 
it was divided to determine ^e area and centroid. Table AI summarizes 
calculations performed. 




^ E Ax _ 113977.5in3 ^ 
J^A 1406.1 in ^ 


= 81 in. 


Figure A2 shows the equivalent rectangular fin for the SDV together with the 
actual and normalized dimensions. 
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Figure Al SDV Fin 


TABLE AI. SDV FIN GRAPHICAL INTEGRATION VALUES 


1 Section 

Area^ (in^) 

X (in) 

Ax (in’) 1 

1 

39.6 

96.8 

3883.3 1 

2 

363.1 

96.8 

35148.1 1 

3 

253.5 

100.0 

25350.0 1 

4 

116.2 

91.3 

10609.1 1 

5 

84.5 

80.5 

6802.3 1 

6 

47.5 

64.3 

3054.3 j 

7 

491.1 

57.8 

28385.6 1 


10.6 

75.0 

795.0 

[ TOTAL 

1406.1 


113977.5 


’ Dimensions based on 3.25 in./side of a square on 
the graph paper 

^ Measured from origin of SDV body-centered axes. 
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I 

i 21.e* 

I 

I 


f 




81' 

Figure A2 


Itnothli -SS* 
spanb-21.6* 

Mpectrailot-jy ^ - .33 
geometric aspect ratio 


SDV Equivalent Fin Dimensions 


dC, 


The value of_L was estimated using Jones' formula 

ac 


=ila=2ii(0.33)= 0.518 
2 2 


dp 

Using the values derived above, the values for (Y^)„ (N^)(, (Y^)„ and (N^)< were 
calculated as follows: 






ap 


* -|(0.032)(0.518)1 = -0.01660 


(N^), = (Y^,x, = (-0.0166) (-0.385) = 0.00639 
(Y^), = x,(Y^, = (-0.385) (-0.0166) = 0.00639 

(N,)j = x^'(Y^), = (-0.385)* (-0.0166) = -0.00246 
The contribution of the fixed fin to the acceleration-related hydrodynamic 


coefficients can be approximated by 




-2jtbA. 


y(a*Tir 


(A5) 
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Substituting the normalized geometric properties of me SDV equivalent fin into 
Equation (A5), the values for (Y^),, (N^)<, (y^V and (N^), were calculated as 
follows: 

rv . . - (-a(3.M)(0.103)(0.032) . 

/Sg*’ ^.332' ♦ 1 

(N^),« x,(Y^, - (-0.385)(-0.01965) « 0.00756 
(Y,),« Xj(Y^),«(-0.385)(-0.01965) - 0.00756 
(N^), - x*(Y^j«(-0.385)'(-0.01965)»-0.00291 
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APPENDIX B: SIMULATION MODEL 


C PROGRAM SIMAUV 

C 

C Fotis A., Papoulias/David C. Warner 

C NAVAL POSTGRADUATE SCHOOL 

C November 1991 

C 

C NPS AUV II Six-Degree-of-Freedom Simulation Model 

C 

C As written, this program uses data files produced by the AJV II 
C during experimental runs. With slight modification the program 
C can use input rudder and speed commands supplied by the 
C programmer to predict AUV II performance for different run 
C profiles 
C 

C DECLARATIONS 
C 

REAL L,MASS,IX,lY,IZ,1X2,IY2,IXY,NU,LRPM,KPDOT,KRDOT,KPQ,KQR 
REAL KVDOT,KP,KR,KVQ,KWP,KWR,KV,KVW,KPN,KDB 

REAL MQDOT,MPP,MPR,MRR,MWDOT,MQ,MVP,MVR,MW,MVV,MDS,MDB,NDRB 
REAL NPDOT, NRDOT,NPQ,NQR,NVDOT,NP,NR,NVQ,NWP,NWR,NV,NVW,NDRS 
REAL MM(6,6),INDX(100) 

DIMENSION X{15),BR(15),HH(15),VECHl(15),VECH2(15),XMMINV(6,6) 
DIMENSION VECV1(15),VECV2(15),F(12),FP(6) 

NPS AUV II GEOMETRIC PROPERTIES (Basic Length Dimension is Feet) 


WEIGHT” 

435.0 

IX 

S 

2.7 

lY 

- 

> 42.0 

12 


45.0 

IXY 

m 

o 

o 

lYZ 

- 

o 

o 

IXZ 

m 

0.0 

L 

- 

87.625/12.0 

RHO 

m 

1.94 

G 

- 

32.2 

XG 

- 

0.125/12.0 

YG 

■i 

O 

o 

ZG 

- 

0.05 

XB 

m 

0.125/12.0 

YB 

- 

o 

o 

ZB 

m 

o 

o 

ZARM 

m 

2.287 

MASS 

-WEIGHT/G 
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BOY -WEIGHT 
XRS —0.377*L 

XRB -+0.283»L 

DRAG COEFFICIENTS AND SPEED CALIBRATION DATA 


CDO 

m 

0.015 

CDY 

m 

0.5 

CDZ 

m 

0.6 

RPMO 

■ 

500.0 

UO 

m 

2.3 


C 

C SURGE HYDRODYNAMIC COEFFICIENTS 
C 

XPP - 0.00000*0.5*RHO*L**4 
XQQ - 0.00000*0.5*RHO*L**4 
XRR —0.01735*0.5*RHO*L**4 
XPR - 0.00000*0.5*RHO*L**4 
' XUDOT —0.00282*0.5*RHO*L**3 
XWQ » 0.00000*0.5*RHO*L**3 
XVP - 0.00000*0.5*RHO*L**3 
XVR - 0.00000*0.5*RHO*L**3 
XQDS - 0.00000*0.5*RHO*L**3 
XQDB - 0.00000*0.5*RHO*L**3 
XRDRS - 0.00000*0.5*RHO*L**3 
XRDR3 - 0.00000*0.5*RHO*L**3 
XW —0.04019*0.5*RHO*L**2 
XWW - 0.00000*0.5*RHO*L**2 
' XVDRS - 0.00000*0.5*RHO*L**2 
XVDRB' - 0.00000*0.5*RHO*L**2 
XWDS - 0.00000*0.5*RHO*L**2■ 

XVTD3 - 0.00000*0.5*RHO*L**2 
XDSDS —0.02345*0.417*0.5*RHO*L**2 
XDBDB —0.02345*0.417*0.5*RHO*L**2 
XDRDR —0.02345*0.417*0.5*RHO*L**2 
XRES - CD0*0.5*RHO*L**2 
XPROP - XRES*(UO/RPMO)»*2 
C ' ' 


C LATERAL HYDRODYNAMIC COEFFICIENTS 
C 

YPDOT - 0.00000*0.5*RHO*L**4 
YRDOT —0.00178*0.5*RHO*L**4 
YPQ - 0.00000*0.5*RHO*L**4 
YQR - 0.00000*0.5*RHO*L**4 
YVDOT —0.03430*0.5*RHO*L**3 
YP - 0.00000*0.5*RHO*L**3 
YR -+0.01187*0.5*RHO*L**3 
YVQ - 0.00000*0.5*RHO*L**3 
YWP - 0.00000*0.5*RHO*L**3 
YWR - 0.00000*0.5*RHO*L**3 
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YV —0.03896*0.5*RHO*L**2 
YVW - 0.00000*0.5*RHO*L**2 
YDRS -+0.02345*0.5*RHO*L**2 
YDRB -+0.02345*0.5*R.HO*L**2 

HEAVE HYDRODYNAMIC COEFFICIENTS 

2QDOT —0.00253*0.5*RHO*L**4 
ZPP - 0.00000*0.5*RHO*L**4 
ZPR - 0.00000*0.5*RHO*L**4 
ZRR - 000000*0.5*RHO*L**4 
' ZWDOT —0.09340*0.5*RHO*L**3 
ZQ —0.07013*0.5*RHO*L**3 
ZVP - 0.00000*0.5*RHO*L**3 
2VR - 0.00000*0.5*RHO*L**3 
ZW —0.15687*0.5*RHO*L**2 
ZW , - 0.00000*0.5*RHO*L**2 
ZDS —0.02345*0.5*RHO»L**2 
ZDB —0.02345*0.5*RHO*L**2 

ROLL HYDRODYNAMIC COEFFICIENTS 


KPDOT 


0.00024*0.5*RHO*L**5 

KRDOT 

- 

0.00000*0.5*RHO*L**5 

KPQ 

- 

0.00000*0.5*RHO*L**5 

KQR 

- 

0.00000*0.5*RHO*L**5 

KVDOT 

a 

0.00000*0.5*PHO*L**4 

KP 

— 

■0.00540*0.5*RHO*L**4 

KR 

m 

0.00000*0.5*RHO*L**4 

KVQ 

- 

0.00000*0.5*RHO*L**4 

KW? 

m 

0.00000*0.5*RKO*L»*4 

KWR 

- 

0.00000*0.5*RHO»L**4 

KV 


0.00000*0.5*RHO*L**3 

KVW 

mm 

■0.00000*0'. 5*RHO*L**3 


PITCH HYDRODYNAMIC COEFFICIENTS 

MQDOT —0.00625*0.5*RHO*L**5 
MP? - 0.00000*0.5*RHO*L**5 
MPR - 0.00000*0.5*PJiO*L**5 
MRP - 0.00000*0.5*RHO*L**5 
MWDOT —0.00253*0.5*RHO*L**4 
MQ —0.03565*0.5*RHO*L**4 
MV? - 0.00000*0.5*RHO*L**4 
MVR- - 0.00000*0.5*RHO*L**4 
MW -+0.05122*0.5*RHO*L**3 
MW - 0.00000*0.5*RHO*L**3 
MDS —0.377*L*YDRS 
MDB , -+0.283*L*YDRB 
C 
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YAW hyp:^odynamic coefficients 

NPDOT -^ 0.00000*0.5*RHO*L**5 
NRDOT —0.00047*0.5*RHO*L**5 
NPQ - 0,00000*0.5*RHO*L**5 
NQR - 0.00000*0.5*RHO*L**5 
NVDOT —0.00178*0.5*RHO*L**4 
NP - 0.0i.000*0.5*RHO*L**4 
NR —0.01022*0.5*RHO*L**4 
NVQ - 0.00000*0.5*RHO*L**4 
NWP - 0.00000*0.5*RHO*L**4 
NWR - 0.00000*0.5*RHO*L**4 
NV —>0.00769*0.5*RHO*L**3 
NVW - 0.00000*0.5*RHO*L**3 
NDRS —0.377*1.* YDRS 
NDRB -+0.283*L*yDRB 

OPEN DATA AND RESULTS FILES. TH6 INPUT FILE (5) IS CHANGED BY 

THE RESEARCHER DEPENDING UPON THE AUV RUN BEING SIMULATED 

OPEN ( 5,FILE-'MODOVAL.DAT',STATUS-'OLD') 

OPEN (11,FILE-'U.RESSTATUS-'NEWM 
OPEN (12,FILE-'V.RES',STATUS-'NEW') , 

OPEN (13,FILE-'W.RESSTATUS-'NEW') 

OPEN (14,FILE-'P.RES',STATUS-'NEW') 

.OPEN (15,FILE-'Q.RES',STATUS-'NEW') 

■OPEN (16,FILE-'R.RES' ,ST.ATUS-'NEW' ) 

OPEN (17,FILE-'DRB.RES',STATUS-'NEW') 

OPEN (i8,FILE-'SSAS.RES',STATUS-'NEW') 

OPEN (19,FILE^'SSAB.RESSTATUS-'NEW') 

OPEN (20,FIL'i>-'PHI .RES', STATUS-'NEW') 

OPEN (21,FILE-'TKETA.RES',STATUS-'NEW') 

OPEN (22,FILE-'PSI.RES',STATUS-'NEW') 

OPEN (23,FILE-'DRS.RES',STATUS-'NEW') 

OPEN (24,FILE-'DS.RES',STATUS-'NEW') 

OPEN (25,FILE='XY.RES',STATUS-'NEW') 

OPEN (26,FILE-'XZ.RES',STATUS-'NEW') 

OPEN (27,FILE-'YZ.RES',STATUS-'NEW') 

OPEN (28,FILE-'ZCELL.RES',STATUS-'NEW' ) 

OPEN (31,FILE-'DRSS.RES',STATUS-'NEW') 

^ OPEN (32,FILE-'DRBS.RES',STATUS-'NEW') 

OPEN (33,FILE-'SIMTODR.RES',STATUS-'NEW') 

MASS MATRIX INITIALIZATION AND DEFINITION 

DO 15 J-1, 6 
DO 10 K-1,6 
XMMINV(J,K)-0'.0 
MM(J,K)-0.0 
10 CONTINUE 
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CONTINUE 

MASS-XUDOT 
M14(l,5)- MASS*ZG 
MM(1,6)—MASS*yG 

MM(2,2)- MASS-YVDOT 
MM(2,4)—MASS*ZG-yPDOT 
MM (2,6)- MASS*XG-YRI30T 

MM(3,3)- MASS-ZWDOT 
MM(3,4)- MASS»YG 
MM(3,5)—MASS*XG-ZQDOT 

MM (4,2) —MAS S * ZG-KVDOT 
MM(4,3)- MASS*YG 
MM(4,4)- IX-KPDOT 
MM(4,5)—IXY 
MM(4, 6)—IXZ-KRDOT 

MM(5,1)- MASS*ZG 
MM(5, 3)—MASS*XG-MWDOT 
MM(5,4)—IXY 
MM(5,5)- lY-MQDOT 
MM(5, 6)—lYZ 

MM(6,1)—MAS£*YG 
MM(6,2)- MASS*XG-NVDOT 
MM(6, 4)—IXZ-NPDOT 
MM{6,5)—lYZ 
MM<6,€)- IZ-NRDOT 

MJ^SS MATRIX INVERSION 

DO 12 I-l, 6 
DO 11 J-1,6 
XMMINVd, J)-0.0 

11 CONTINUE 
XMMINVd, I)-! .0 

12 CONTINUE 

CALL INVTA<MM,6,INDX,D) 

DO 13 J«l,6 

CALL IN'/TB (MM, 6, INDX, XMMINVd, J) ) 

13 , CONTINUE 
C 

C VARIABLE INITIALIZATION 
C 

TWOPI -8.0*ATANd.0) 

PI -0.5*TWOPI 
lECHO -10 
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IPRNT -1 
JPRNT -0 
IJ -O 

JE -0 

DELTA -0.1 
ISIM -10000 

DEFXNE THE LENGTH (X), BREADTH (DR), AND HEIGHT (HH) TERMS 


X(l) 


-43.9/12.0 

X(2) 

- 

-39.2/12.0 

X(3) 

m 

-35.2/12.0 

X(4) 

m 

-31.2/12.0 

X(5> 

- 

-27.2/12.0 

X(6) 

- 

-10.0/12.0 

X{7) 

- 

0.0/12.0 

X(8) 

m 

10.0/12.0 

5C(9) 

m 

26.8/12.0 

X(10) 

- 

32.0/12.0 

Xdl) 

a 

37.8/12.0 

X(12) 

-. 

40.8/12.0 

X(13) 


42.3/12.0 

X(14) 


43:3/12.0 

X(15) 

m 

43.7/12.0 

HH(1) 

m 

0.0/12.0 

HH (2) 

m 

2.7/12,0 

HH(3) 

m 

5.2/12.0 

HH (4) 

m 

7.6/12.0 

HH(5) 

m 

10.1/12.0 

HH(6) 


10.1/12.0 

HH(7) 

- 

10.1/12,0 

HH(8) 

m 

10.1 12.0 

HH(9) 

m 

10.1/12.0 

HH(10) 

- 

9.6/12.0 

HH(11) 

m 

7,6/12.0 

HH(12) 

- 

5.6/12.0 

HH(13) 

- 

4.2/12.0 

HH (14) 

m 

2.3/12.0 

HH(15) 

- 

0.0/12.0 

BR(1) 

m 

,16.5/12.0 

BR(2) 

- 

16.5/12.0 

BR(3) 

- 

16.5/12.0 

BR(4) 

- 

16.5/12.0 

BR(5) 


16.5/12.0 

BR(6) 

- 

16.5/12.0 

BR(7) 

- 

16.5/12.0 

BR(8) 

■1 

16.5/12.0 

BR(9) 

m 

16.5/12.0 
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BR(IO)- 

15.5/12.0 

BR(ll)- 

12.4/12.0 

BR(12)- 

9.5/12.0 

BR(13)- 

■ 7.0/12.0 

BR(14)- 

4.0/12.0 

BR(15)- 

0.0/12.0 


RUDDER STALL ANGLES 

STL1-25.0*PI/180.0 
STL2-30.0*PI/180.0 
STL3-45.0*PI/180.0 

SIMULATION BEGINS 

DO 100 1-1,ISIM 
C 

C Some early kXTJ 11 files had an ,ERR1 field after RANGEl, and 
C an ERR2 field after RANGE2. If the file being used has these 


fields. 

the following 

Statement must be modified. 

READ ' 

(5,*,END-500) 

TIME,XPOSE,YPOSE,ZPOSE,PHIE,THETAE,PSIE, 

t 


PE,QE,RE,DRE,DSE,RANGEl, RANGE2, 

fi 

' 

SPEEDS,RPMORD,RRPM,LRPM 

IF (I 

.NE.l) GO TO 

111 

XPOS 

- XPOSE 


ypcs 

- YPOSE 

, 

ZPOS 

- ZPOSE 


PHI 

- PHIE 


THETA 

- THETAE 


P 

- PE 


Q 

- QE 


R 

- RE 


DRS - 

DRE 


DRB = 

-DRE 

' • ’ 

DS •= 

DSE 

• 

DB * 

-DSE 



RPM= (LRPM-^RRPM) / 2 , 

C CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
C , 

DO 600 K=l, 15' 

UCF=(V+X(K)^R)**2+<W-X(K)*Q )**2 
UCF=SQPT(UCF) 

IF {UCF.LT.l.E-6) GO TO 601 

CFLOW =*CDY*HH{K) * (V+X(K) ‘R) **2+CDZ*BR(K) * (W-X{K) *Q> **2 
VECHl(K)=CFLOW*(V+X(K)*R)/UCF 
VECH2(X)»CFLOW»(V+X(K)*R)*X(K)/UCF 
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VECV], (K) -CFLOW* (W-X (K) *Q) /UCF 
VECV2(K)-CFLOW*(W-X(K)*Q)*X(K)/UCF 
600 CONTINUE 

CALL TRAP(15,VECVI,X,HEAVE) 

CALL TRAP (15, \’SCV2,X, PITCH) 

CALL TRAP(15,VECH1,X,SWAY ) 

CALL TRAP(15,VECH2,X,YAW ) 

HEAVE—0.5»RHO*HEAVE 
PITCH»+0.5*RHO»PITCH 
SWAY —0i5»RHO*SWAY 
YAW —0.5*RHO*YAW 
BETA *ATAN(ABS(V)/U) 

IF (R.NE.0.0) RADIUS-SQRT(U**2+V**2)/R 
IF (R.EQ.0.0) RADIUS-200.0*L 

ARS -ABS(XRS)*COS(BETA)/'RADIUS+ABS(XRS)*SIN(BETA)) 

ARP -ABS(XRB)*COS(BETA)/(RADIUS+ABS(XRB)*SIN(BETA)) 

XRS -ATAN(ABS(ARS)) 

ARB -ATAN(ABS(ARB)) 

ARS/ -0.0 
ARB -0.0 

SSAS -ATAN(V/U)+ARS 
SSAB -ATAN(V/U)-ARB 
UV -U 
C 

SSAS-0.0 

SSAB-C.O 

GO TO 602 

.601 HE.AVE-0.0 ^ 

PITCK-0.0 
SWAY -0.0 
YAW -0.0 

SSAS =0.0 ' 

SSAB -0.0 ' 

UV >=U 

602 CONTINUE j 

c 

c FORCE EQUATIONS 
C 

C SURGE FORCE 
C 

FP(1) = MASS»V*R-MASS*W*Q+MASS*XG*Q**2+MASS*XG*R**2- 
£ MASS*YG*F*Q-MASS*ZG*P*R+XPP*P**2+XQQ*Q**2+XRR*R**2+ 

£ XPR»P*R+XWQ»W*Q+XVP*V*P+XVR*V*R+U*Q*(XQDS*DS+XQDB*DB)+ 

£ U*R*(XRDRS*(DRS-SSAS)+XRDRB*(DRB-SSAB))+XVV*V**2+ 

£ XWW*W**2+U*V*(XVDRS*(DRS-SSAS)+XDRB*(DRB-SSAB))+ 

£ U*W*(XWDS*DS+XWDB*DB)+(XDSDS*DS**2+XDBDB*DB**2+ 

£ XDRDR*((DRS-SSAS)**2+(DRB-SSAB)**2))*UV**2- 

£ (WEIGHT-BOY)*SIN(THETA)+XPROP*RPM*RPM-XRES*U*U 

C 
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SWAY FORCE 

SFDRS-YDRS*UV**2*DRSS 

SFDRB»YDRB*UV**2*DRBS 

FP{2) - -MASS^U*R-MASS*XG*P*Q+MASS*yG*R*'*2-MASS*ZG*Q*R+ 

& YPQ»P*Q+YQR*Q*R+YP*U*P+YR*U»R+YVQ*V*Q+YWP*W*P+yWR*W*R+ 

& YV*U*V+YVW*V*W+SFDRS+SFDRB+(WEIGHT-BOY)• 

£ COS(THETA)*SIN(PHI)+MASS*W*P+MASS*YG*P**2+SWAY 

HEAVE FORCE 

FP(3) - MASS*U*Q-MASS*V*1?-MASS*XG*P*R-MASS*YG*Q*R+ 

£ MASS*2G*P**2+MASS*ZG*Q**2+ZPP*P’**2+ZPR*P*R+2RR*R**2+ 

£ ZQ*U*Q+ZVP*V*P+ZVR*V*R+ZW*U*W+ZW*V**2+HEAVE+ 

£ U**2*(ZDS*DS+ZDB*DB)+(WEIGHT-BOY)*COS(THETA)*COS(PHI) 

ROLL MOMENT 

FP(4) » -IZ*Q*R+IY*Q*R-IXY*P*R+IYZ*Q**2-IYZ*R**2+IXZ*P*Q+ 

£ MASS*YG*U*Q-MASS*YG*V*P-MASS*ZG*W*P+KPQ*P*Q+KQR*Q*R+ 

£ KP*U»P+KR*U*R+KVQ»V*Q+KWP*W*P+KWR*W*R+KV*U*V+KVW*V*W+ 

£ (YG*WEIGHT-YB*BOY) *COS (THETA) *COS (PHI) {ZG*WEIGHT- 

£ ZB*BOY)*COS(THETA)‘SIN(PHI)+MASS*ZG*U*R 

PITCH MOMENT 


FP(5) f -IX*P»R+I2*P*R+IXY*Q*R-IYZ*P*Q-IXZ*P**2+IXZ*R**2- 
£ MASS*XG*U*Q+MASS*XG*V*P+MASS*ZG*V*R-MASS*ZG*W*Q+ 

£ MPP*P**2+MPR*P’‘R+MRR*R**2+MQ*U»Q+MVP*V*P+MVR*V*R+MW*U»W+ 

£ MW*V**2+U**2*(MDS*DS+MDB*DB)-(XG*WEIGHT- 

£ XB*BOY)*COS(THETA)*COS(PHI)- 

£ (ZG*WEIGHT-ZB*BOY)*SIN(THETA)+PITCH 

YAW MOMENT 

YMDRS=Nt)RS*UV**2»DRSS 
YMDRB=NDRB*UV**2*DRBS 

FP(6) * -IY*P»Q+IX*P*Q+IXY*P**2-IXY*Q**2+IYZ*P*R-IXZ*Q*R- 
£ KASS*XG*U»R+MASS*XG*W*P-MASS*YG»V*R+MASS*YG*W*Q+NPQ*P»Q+ 

£ NQR»Q*R+NP*U*P+NR*U»R+NVQ*V*Q+NWP*W*P+NWR*W*R+NV*U»V+ 

£ NVW*V*W+YMORS+YMDRB+(XG*WEIGHT-XB*BOY)* 

£ COS(THETA)*SIN(PHI)+{YG*WEIGHT-YB*BOY)*SIN(THETA)+YAW 

COMPUTE THE RIGHT HAND SIDE QF XbOT=F(X) 

DO 610 J = 1,6 
F(J) = 0.0 
DO 611 K = 1,6 

F(J) = XMMINV(J,K)*FP(K) + F(J) 

611 CONTINUE 
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€10 CONTINUE 

COMPUTE INERTIAL POSITION RATES 

F{7) - U*COS (PSD *COS (THETA)+V* (COS (PSD 'SIN (THETA)* 

£ SIN (PHI) -SIN (PSD *COS (PHI) ) +W* (COS (PSD *SIN (THETA) * 

£ COS(PHD+SIN(PSD*SIN(PHD) ' 

F(8) - U*SIN (PSD *COS (THETA)+V* (SIN (PSD *SIN (THETA) * 

£ SIN (PHD+COS (PSD *COS (PHI) )+W* (SIN (PSD *SIN (THETA) * 

£ , COS (PHI)-COS (PSD *SIN (PHI) ) 

F(9) - -U*SIN(THETA)+V*COS(THETA)*SIN(PHI)+W*COS(THETA)* 

£ COS (PHI) 

COMPUTE EULER ANGLE RATES 

I 

F(10)- P+Q*SIN(PHD*TAN(THETA)+R*COS(PHI)*TAN(THETA) 

C ■ ■ 

F(1D= Q*COS (PHD-R*SIN(PHD 
C 

F (12) = Q*SIN (PHD/COS (THETA)+R*COS (PHD/CCS (THETA) 

C 

C ASSIGN VALUES TO THE "XDOT" VECTOR 

C . . 


UDCT 

« 

F(l) 

VDCT 

at 

F (2) 

WDOT 

- 

F(3) 

PDOT 

- 

F(4) 

QDOT 

m 

F(5) 

RDOT 

= ' 

F(6) 

XDOT 

- 

F(7) 

YDOT 

3M 

F(8) 

ZDOT 

- 

F(9) 

PHIDOT. 

m 

F(10) 

THEDOT 

s 

F(ll) 

PSIDOT 


F(12) 


C FIRST ORDER INTEGRATION 


U 

* 

u 

+ 

DELTA*UDOT 

V 


V 

+ 

DELTA*VDOT 

W 

9B 

w 

+ 

DELTA*WDOT 

F 

- 

,p 

+ 

DELTA»PDOT 

Q 

•= 

Q 

+ 

DELTA*QD01 

R ' 

at 

R 

+ 

DELTA*RDOT 

XPOS 

- 

XPOS 

+ ' 

DELTA*XDOT 

YPOS 

- 

YPOS 

+ 

DELTA*YDOT 

ZPOS 


ZPOS 

+ 

DELTA*ZDOT 

PHI 

* 

PHI 

+ 

DELTA*PHIDOT 
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THETA - THETA + DELTA*THEDOT 
C 

C DEPTH CELL READING 
C 

2CELL - ZPOS + ZARM*SIN(THETA) 

C 

C PRINT AND ECHO RESULTS 
C 

jr-JE+1 

IF (JE.NE.IECHO) GO TO 99 
WRITE (*,*) TIME,PSI 
JE-0 

99 JPRNT-JPRNT+1 

IF (JPRNT.NE.IPRNT) GO TO 100 
WRITE (11,*), TIME,U 

WRITE (12,*) TIME,ATAN(V/U)*180,0/PI 
WRITE (13,*) TIME,ATAN(W/U)*180.0/PI 
WRITE (14,*) TIME,P*180.0/PI 
WRITE (15,*) TIME,Q*180.0/PI 
WRITE (16,*) TIME,R*180.0/PI 
WRITE (17,*) TIME,DRB*180.0/PI 
WRITE (18,*) TIME,(DRS-SSAS)*180.0/PI 
WRITE (19,*) TIME,(DRB-SSAB)*180.0/PI 
WRITE (20,*) TIME,PHI*180.0/Pl' 

WRITE (21,*) TIME,THETA*180,0/PI 
WRITE (22,*) TIME,PSI*18b.0/Pl,PSIl*180.0/PI 
WRITE (23,*) TIME,DRS*180.0/PI 
WRITE (24,*) TIME,DS*180.0/PI 
WRITE (25,*) XPOS,YPOS 
WRITE (26,*) XPOS,ZPOS 
WRITE (27,.*) YPOS,2POS 
WRITE (28,*) TIME,2CELL 
WRITE (31,,*) TIME,DRSS*180.0/PI 
WRITE (32,*) T.:ME,DRBS*180.0/PI 
WRITE (33,900),TIME,XPOS,YPOS,PSI,U,DR,R 
900 FORMAT (7F10.5) 

JPRNT-0 

100 CONTINUE 
500 STOP 

end' 

SUBROUTINE TWVP (N, A, B, OUT) 

C ' 

C NUMERICAL INTEGRATION ROUTINE USING THE TRAPEZOIDAL RULE 
C 

DIMENSION A(l),B(1) 

N1=N-1 
OUT=0.0 
DO 1 I-1,N1 

OUT1*0.5*(A(I)+A(I+1))*(B(I+1)-B(I)) 
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OUT -OUT+OUTl 
CONTINUE 
RETURN 
END 

C— 

SUBROUTINE INVTA(MM,N,INDX,D) 

MATRIX INVERSTION ROUTINE FOR MATRIX A 

PARAMETER (NMAX-100,TINY-1.OE-20) 

DIMENSION INDX(6) , W(NMAX) 

REAL MM(6,6) 

D-1 

DO 12 I«1,N 
AAMAX-0. 

DO 11 J-1,N 

IF(ABS(MM(1,J)).GT.AAMAX) AAMAX-ABS(MM(I,J)) ■ 

11 CONTINUE 

IF (AAMAX.EQ.O.) PAUSE 'SINGULAR MATRIX' 
W(I)*1./AAMAX 

12 CONTINUE 

DO 19 J=1,N 
DO 14 I-1,J-1 
SUM=MM(I,J) 

. ..DO 13 K=l, I-l 

SUK=SUM-MM(I,K)•MM(K,J) 

13 CONTINUE 
MM(I,J)=SUM 

14 CONTINUE 
AAMAX=0. 

DO 16 I-J,N 
■ SUM“MM(I,J) 

DO 15 K=1,J-1 

SUM=SUM-MM(I,K)*MM(K, J) 

15 CONTINUE 
MM(I,J)*SUM 
DUM*=W(I) *ABS (SUM) 

IF (DUM.GE.AAMAX) THEN 
IMAX=I 
AAMAX=DUM 
ENDIF 

16, CONTINUE 

IF (J.NE.IMAX)THEN, 

DO 17 K=1,N 
DUM=MM(IMAX,K) 

MM(IMAX,K)«MM(J,K) 

MM(J,K)-DUM ' 

17 CONTINUE 

D=-D 

W(IMAX)*W(J) 
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ENDIF 

INDX(J)-IMAX 

IF{MM{J, J) .EQ.O.)MH(J, J)-TINJf 
IF(J.NE.N)THEN 
DUM-1./MM(J,J) 

DO 18 

MM{I, J) *DUM 

18 CONTINUE 
ENDIF 

19 CONTINUE 
RETURN 
END 

C— 

SUBROUTINE INVTB(MM,N,INDX,B) 

MATRIX INVERSION ROUTINE FOR MATRIX B 

DIMENSION, INDX(N),B(N) 

REAL MM(6,6) 

II-O. 

DO 12 :-l,N 
LL-INDX(I) 

SUM-B(LL) 

B(LL)-B(I) 

IF (II.NE.O)THEN 
DO 11 J=II,I-1 
SUM-SUM-MM(I,J)*B(J) 

11 CONTINUE 

ELSE IF (SUM.NE.O) THEN 
II»I 
ENDIF 
B(I)»SUM 

12 CONTINUE 

DO 14 I-N,l,-1 
SUM=B(I) 

IF (,I.LT.N)THEN 
■ DO 13 J-I+1,N , 

SUM=SUM-MM(I,J)*B(J) 

13 CONTINUE 
ENDIF 

B(I)-SUM/MM(I, I) 

14 CONTINUE 
RETURN 
END 
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APPENDIX G PROGRAM OBSERVE 




C PROGRAM OBSERVE 

C 

C Fotis A Papoulias/David Warner 

C NAVAL POSTGRADUATE SCHOOL 

C 28 October 1991 

C 

C This program uses the results of GIMCH6 for the AUV simulation of 
C the September MODOVAL computer run. The only inputs are TIME, 

C XPOSE, YPOSE, PSIE, UE, DRE, RE. Outputs are a simulated track 
C ("Truth") plotted from the given XPOS and YPOS, a track that the 
C AUV II would use as its DR (without V>, and the Enhanced 
C Position obtained by using a reduced order observer to estimate 
C value of lateral motion, V. 

C 

REAL L,MASS,NRDOT,NVDOT,NR,NV,NDRS,NDRB,IZ 
REAL KK,KV 

DIMENSION X(15) ,HH(15>,BR(15),VEC1(15),VEC2(15) 

C , , • 

C GEO.METRIC PROPERTIES 
C 

WEIGHT-435.0 
12 -45.0 

L . -87.625/12.0 

RHO -1.94 ' 

G -32.2 
XG . -0.0/12.0 

CDO -0.015 
CDY ■ -0.5 

CDZ =0.6 ' 

RPMO -550 
, UO -2.5 
MASS -WEIGHT/g 
XRS —0.377*L 
XRB =+0.238*L. 

SURGE HYDRODYNAMIC COEFFICIENTS 

XRR —0.01735*0.5*RHO*L**4 
XUDOT—0.00282*0.5*RHO*L**3 
XW —0.04019*0.5*RHO*L**2 
XDSD.S—0.02345*0.417*0.5*RHO*L**2 
XDRDB—0.02345*0.417*0.5*RHO*L* *2 
XDRDR—0.02345*0.417*0.5*RHO*L**2 
XRES =CDO*0.5*RHO*L»*2 
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XPPOP-XRES*(UO/RPMO)**2 

LATERAL HYDRODYNAMIC COEFFICIENTS 

YRDOT—0,00178*0.5*RHO*L**4 
YVDOT—0.03430*0.5*RHO*L**3 
YR -+0.01187*0.5*RHO*L**3 
YV —0.03896*0.5*RHO*L**2 
YDRS -+0.02345*0.5*RHO*L**2 
YDRB -+0.02345*0.5*RHO*L**2 

YAW HYDRODYNAMIC COEFFICIENTS 

NRDOT—0.00047*0.5*RHO*L**5 
NVDOT—0.0017 8*0.5*RHO*L**4 
NR —0.01022*0.5*RHO*L**4 

NV —0.00769*0.5*RHO*L**3 

' NDRS —0.377*YDRS*L 
NDRB -+0.283*YDRB*L 

THE FILE USED IN THE FIRST STATEMENT WILL VARY DEPENDING UPON 
THE RUN BEING ANALYZED AND THE SOURCE OF DATA 

OPEN {10,FILE=*'SIMTODR.RES', STATUS-'OLD' ) 

OPEN (11,FILE='XY1.RES',STATUS*'NEW') 

OPEN ( 12 ; FILE*'XY2.RES', STATUS-' NEW') 

OPEN (13,FILE-'XY3.RES',STATUS-'NEW') 

OPEN (14,FILE-'LAT.RES',STATUS-'NEW') 

OPEN (20,.FILE-'TRUAUVl.RES',STATUS-'NEW') 

OPEN (21,FILE='TRUAUV2.RES',STATUS-'NEW') 

VARIABLE INITIALIZATION 

ISIM-IOOOC 

TWOPI -8.0*ATAN(1.0) 

PI -0.5*TVIOPI 
lECHO =10 
IPRNT =1 
JPRKT -0 
IJ -0 
■ JE -0 

DELTA =0.1 ■ . . 

V-0.0 

c . ■ ' 

C THE INITIAL AUV II POSITION IN THE POOL MUST BE ENTERED BASED 
C ON KNOWN RANGE INFORMATION, FROM THE DATA FILE OR OTHER SOURCE. 

C DWELL TIME AT THE BEGINNING OF THE RUN MUST ALSO BE ENTERED TO 
C PROPERLY CORRECT PSI FOR GYRO DRIFT. 

C ONCE THE INITIAL RANGES ARE ENTERED THE INITIAL AUV II POSITION 
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C IN THE POOL IS COMPUTED Bi^SED ON THE LENGTH (LPOOL) AND WIDTH 
C (WPOOL) OF THE POOL. 

C 

WRITE {*,*) 'ENTER INITIAL RANGE FROM LEFT SONAR SENSOR 
READ (*,*) RLRNG 

WRITE (*,•) 'ENTER INITIAL RANGE FROM FORWARD SONAR SENSOR 
READ (*,*) RFRNG 
WRITE (*,*) 'ENTER DWELL TIME 
READ (•,*) TDWELL 
WPOOL-60 
LPOOL-117 

XSTART-LPOOL-RFRNG 
YSTART-WPOOL-RLRNG 

COMPUTE THE COEFFICIENTS OF THE A AND B MATRICIES 

Al-MASS-YVDOT 
A2-MASS*XG-YRDOT 
A.3-YR-MASS 
A4-YV 
AS-YDRS 
. A6«YDRB ' 

Bl»MASS*XG-NVDOT , , 

B2-12-NRDOT 
B3«NR-MASS»XG 
B4=NV 
B5»=NDRS 
B6“NDRB 

DENIAL*B2-A2*B1 

AA11«(A4*B2-A2*B4)/DEN 
AA:2-(A3*B2-A2*B3)/DEN 
AA21=(A4*B1-A1*B4)/(-DEN) 

AA22=(A3*B1-A1*B3)/(-DEN) 

BB11=(A5*B2-A2*B5)/DEN 
BB12= (A6*B2-A2*B6)/DEN 
BB21-(A5*B1-A1*B5)/(-DEN) 

BB22=(A6*B1-A1*B6)/(-DEN) 

BB1-BB11-BB12 
BB2-BB21-BB22 

ENTER THE TIME CONSTANT OF THE LATERAL MOTION OBSERVER 

WRITE (*,*) ' ENTER V-OBSERVER TIME CONSTANT' 

READ (»,*) TV03S 
GAIN*(AAll+1.0/(TVOBS*L))/AA21 
WRITE <*,*) GAIN 
C 

IJ-0 
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JE-0 

SIMULATION BEGINS 
DO 100 I-1,ISIM 

THIS PROGRAM BE USED TO EITHER USE DATA FILES WRITTEN DURING 

AUV II TRIAL RUNS, OR A SEPARATELY GENERATED DATA FILE. 

Some early AUV II files had an ERRl field after RANGEl, and 
an ERR2 field after J^NGE2. If the file beingg used has these 
fields, the following statement must be modified. 

READ (10,*,END-500) TIME,XPOSE,YPOSE,ZPOSE,PHIE,THETAE,PSIE, 
£ PE,QE,RE,DRE,DSE,RANGEl,RANGE2, 

S' ■ SPEEDE,RPMORD,RRPM,LRPM 

THIS STATEMENT IS USED TO READ A SEPARATELY GENERATED FILE OF 
DATA FROM SIMAUV 

READ (10,*,END-'500) TIME, XPOSE, YPOSE, PSIE, SPEEDS,DRE, RE 

The following statements transfer the actual AUV II data to 
variables used in the computation of various positions. The 111 
jump statement is used to establish initial positions for only 
the first time step. 

XiPOS,Y1P0S,PSIMX,PSIMY: Track position for simulated AUV II 
X2POS,Y2?OS,PNOVX,PNOVY: Track position for track without V 
X3POS,Y3POS,POSDRX,POSDRY: Track position as determined using 
Reduced order observer 

IF (I.NE.l) GO TO 111 
XIPOS - XPOSE 
YIPOS » YPOSE 
X2POS - XPOSE 
Y2POS - YPOSE 
X3POS « XPOSE 
Y3POS » YPOSE 
11 PSI - PSIE 

U - SPEEDE 
R - RE 

These statements apply actual rudder positions to the DR proram 

DRS - DRE 
DRB *-DRE 

C These statements calculate the reduced order observer "constants" 
C 

IF (U .EQ. 0.0) GO TO 1009 
KK - (BB1*AA21-BB2*AA11)*U 
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T3 - BB2/KK 

KV (BB2*AA12-BB1*AA22) *17 

T4 - BBl/KV 

C 

C The Simulated AUV II position is provided from the input data 
C 

XIPOS-XPOSE 

YIPOS-YPOSE 

C 

C REDUCED ORDER OBSERVER FOR V 
C 

OVDOT-(AAl1‘U-GAIN*AA21*U)*OV+{AA12 «U-GAIN*AA22 *U+ 

4 (AA11*U-GAIN*AA21*U)*GA1N)*R+(BB11-GAIN*BB21)* 

4 U*U»DRS+(BB12-GAIN*BB22)*U*U*DRB 

OV -OV+DELTA*OVDOT 
VHAT -GAIN»R+OV 
VI -VHAT 

X3DOT-U*COS(PSIE)-V1*S1N(PSIE) 

Y3DOT-U*SIN(PSIE)+Vl*qOS(PSIE) 

X3?OS-X3POS+DELTA*X3DOT 

Y3POS-Y3POS+DELTA*Y3DOT 

c ^ ' 

C Calculate the Simulated AUV li postion without V information 
C 

X2DOT -U*C0S(PS1E) 

Y2DOT -U*SIN{PSIE) 

X2POS-X2'POS+DElTA»X2DOT 
Y2POS»Y2POS+DELTA*Y2DOT 
1009 CONTINUE 
C 

C NOW THAT ALL THE X 4 Y POSITIONS HAVE BEEN CALCULATED, TRANSFORM 
C THE POSITIONS INTO POOL COORDINATES 
C 

C "TRUE" SIMULATED POSITION 

C ' ' 

PSIMX-XSTART+XIPOS' 

PSIMY-YSTART-YIPOS 

C 

C SIMULATED AUV II POSITION, WITHOUT V INFORMATION 
C , . , 

PNOVX-XSTART+X2POS 

PNOVY-YSTART-Y2POS 

C 

C ENHANCED POSITION ESTIMATE 
C 

POSDRX-XSTART+X3POS 
POSDRY-YSTART-Y3POS 
JE-JE+1 . 

IF (JE.NE.IECHO) GO TO 99 
WRITE (*,*) TIME 
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JE-0 

99 J-J+1 

IF (J.NE.IPRNT) GO TO 100 

IJ-l’j+1 

TlME-1‘DELTA 

WRITE (11,*) X1P0S,Y1P0S 

WRITE (12,*) X2POS,Y2POS 

WRITE (13,*) XXPOS,Y3POS 

WRITE (14,*) V,V1 

WRITE (20,*) TIME,PSIMX,PSIMY,PNOVX,PNOVY 
WRITE (21,*) POSDRX,,POSDRY,PSI,PSlE 
J-0 

100 CONTINUE 
500 STOP 

END 
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